diff --git a/orange_ros2.rosinstall b/orange_ros2.rosinstall index 9d570c5..633e5bc 100644 --- a/orange_ros2.rosinstall +++ b/orange_ros2.rosinstall @@ -42,3 +42,7 @@ local-name: livox_laser_simulation_RO2 uri: https://github.com/stm32f303ret6/livox_laser_simulation_RO2.git version: main +- git: + local-name: serial + uri: https://github.com/RoverRobotics-forks/serial-ros2.git + version: master diff --git a/orange_sensor_tools/CMakeLists.txt b/orange_sensor_tools/CMakeLists.txt index 7b6a0b3..e819ed2 100644 --- a/orange_sensor_tools/CMakeLists.txt +++ b/orange_sensor_tools/CMakeLists.txt @@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +cmake_policy(SET CMP0074 NEW) + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -12,12 +14,21 @@ find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(laser_geometry REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(pcl_ros REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) +find_package(serial REQUIRED) + +# add include directories +include_directories( + include +) set(dependencies rclcpp @@ -46,9 +57,29 @@ install(TARGETS laserscan_multi_merger RUNTIME DESTINATION lib/${PROJECT_NAME} ) +add_executable(led_node src/led_node.cpp) +ament_target_dependencies(led_node rclcpp std_msgs serial) + +install(TARGETS led_node + DESTINATION lib/${PROJECT_NAME}) + +add_executable(wheel_imu_odom src/wheel_imu_odom.cpp) +ament_target_dependencies(wheel_imu_odom + rclcpp + sensor_msgs + nav_msgs + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +install(TARGETS wheel_imu_odom + DESTINATION lib/${PROJECT_NAME}) + # install -install(DIRECTORY src launch config +install(DIRECTORY config firmware include launch src DESTINATION share/${PROJECT_NAME}/ ) -ament_package() \ No newline at end of file +ament_package() diff --git a/orange_sensor_tools/firmware/LED.ino b/orange_sensor_tools/firmware/LED.ino new file mode 100644 index 0000000..b63e8b8 --- /dev/null +++ b/orange_sensor_tools/firmware/LED.ino @@ -0,0 +1,70 @@ +#include + +#define LED_PIN 6 +#define NUMPIXELS 30 +#define BUFFER_SIZE 10 + +Adafruit_NeoPixel pixels(NUMPIXELS, LED_PIN, NEO_GRB + NEO_KHZ800); + +char receive_cmd[BUFFER_SIZE]; +uint8_t cmd_idx = 0; + +bool robot_state = false; +bool led_state = false; +unsigned long prev_millis = 0; +const long interval = 500; // 500ms +bool led_on = false; + +void setup() { + Serial.begin(115200); + pixels.begin(); + pixels.clear(); + pixels.show(); +} + +void loop() { + while (Serial.available()) { + char c = Serial.read(); + if (c == '\n') { + receive_cmd[cmd_idx] = '\0'; + processCommand(receive_cmd); + cmd_idx = 0; + } else if (cmd_idx < BUFFER_SIZE - 1) { + receive_cmd[cmd_idx++] = c; + } + } + + unsigned long curr_millis = millis(); + if (robot_state) { // ON + if (led_state) { // FLASH + if (curr_millis - prev_millis >= interval) { + prev_millis = curr_millis; + led_on = !led_on; + updateLED(led_on); + } + } else { // SOLID + updateLED(true); + } + } else { // OFF + updateLED(false); + } +} + +void processCommand(const char* command) { + if (strcmp(command, "ON") == 0) { + robot_state = true; + } else if (strcmp(command, "OFF") == 0) { + robot_state = false; + } else if (strcmp(command, "FLASH") == 0) { + led_state = true; + } else if (strcmp(command, "SOLID") == 0) { + led_state = false; + } +} + +void updateLED(bool state) { + for (int i = 0; i < NUMPIXELS; i++) { + pixels.setPixelColor(i, state ? pixels.Color(55, 0, 0) : pixels.Color(0, 0, 0)); + } + pixels.show(); +} diff --git a/orange_sensor_tools/include/led_node.hpp b/orange_sensor_tools/include/led_node.hpp new file mode 100644 index 0000000..28e9a31 --- /dev/null +++ b/orange_sensor_tools/include/led_node.hpp @@ -0,0 +1,37 @@ +#ifndef LED_NODE_HPP +#define LED_NODE_HPP + +#include + +#include +#include +#include + +class LedController : public rclcpp::Node +{ +public: + LedController(); + ~LedController(); + +private: + void navCallback(const std_msgs::msg::Bool::SharedPtr msg); + void estopCallback(const std_msgs::msg::Bool::SharedPtr msg); + void sendSerialCommand(); + + rclcpp::Subscription::SharedPtr nav_sub_; + rclcpp::Subscription::SharedPtr estop_sub_; + //rclcpp::Publisher::SharedPtr led_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + serial::Serial serial_port_; + + std::string port_name_; + int baud_rate_; + std::string nav_topic_; + std::string estop_topic_; + bool nav_state_; + bool estop_state_; + std::string led_state_; +}; + +#endif // LED_NODE_HPP diff --git a/orange_sensor_tools/include/wheel_imu_odom.hpp b/orange_sensor_tools/include/wheel_imu_odom.hpp new file mode 100644 index 0000000..bc391ca --- /dev/null +++ b/orange_sensor_tools/include/wheel_imu_odom.hpp @@ -0,0 +1,67 @@ +#ifndef WHEEL_IMU_ODOM_HPP +#define WHEEL_IMU_ODOM_HPP + +#include +#include + +#include +#include +#include +#include +#include + +class OdomFusionNode : public rclcpp::Node +{ +public: + OdomFusionNode(); + +private: + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + static void getYawFromQuaternion( + const geometry_msgs::msg::Quaternion & q, double & roll, double & pitch, double & yaw); + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr fused_odom_pub_; + std::shared_ptr tf_broadcaster_; + + std::string imu_topic_; + std::string odom_topic_; + std::string fused_odom_topic_; + + std::string odom_header_frame_; + std::string odom_child_frame_; + std::string TF_header_frame_; + std::string TF_child_frame_; + + double scale_factor_; + double pitch_diff_th_; + bool publish_odom_; + bool publish_TF_; + bool debug_; + + rclcpp::Time prev_time_; + bool imu_received_; + bool baseline_initialized_; + + double roll_; + double pitch_; + double yaw_; + double baseline_pitch_; + + double position_x_; + double position_y_; + double position_z_; + + double orientation_x_; + double orientation_y_; + double orientation_z_; + double orientation_w_; + + double ang_vel_x_; + double ang_vel_y_; + double ang_vel_z_; +}; + +#endif // WHEEL_IMU_ODOM_HPP diff --git a/orange_sensor_tools/package.xml b/orange_sensor_tools/package.xml index 5270b22..9f5f73b 100644 --- a/orange_sensor_tools/package.xml +++ b/orange_sensor_tools/package.xml @@ -7,20 +7,25 @@ Shibuya Kubota Mori + Saito Apache-2.0 ament_cmake sensor_msgs libpcl-all-dev pcl_ros std_msgs + nav_msgs + geometry_msgs tf2 tf2_ros + tf2_geometry_msgs laser_geometry rclcpp rclcpp_action rclcpp_lifecycle pointcloud_to_laserscan robot_localization + serial rosidl_default_runtime linefit_ground_segmentation_ros diff --git a/orange_sensor_tools/src/led_node.cpp b/orange_sensor_tools/src/led_node.cpp new file mode 100644 index 0000000..be1bc0a --- /dev/null +++ b/orange_sensor_tools/src/led_node.cpp @@ -0,0 +1,73 @@ +#include "led_node.hpp" + +using namespace std::chrono_literals; + +LedController::LedController() : Node("LED_node"), nav_state_(false), estop_state_(false) +{ + port_name_ = this->declare_parameter("serial_port", "/dev/sensors/LED"); + baud_rate_ = this->declare_parameter("baud_rate", 115200); + nav_topic_ = this->declare_parameter("nav_topic", "/nav_state"); + estop_topic_ = this->declare_parameter("estop_topic", "/estop"); + + try { + serial_port_.setPort(port_name_); + serial_port_.setBaudrate(baud_rate_); + serial::Timeout to = serial::Timeout::simpleTimeout(1000); + serial_port_.setTimeout(to); + serial_port_.open(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", e.what()); + } + + nav_sub_ = this->create_subscription( + nav_topic_, 10, std::bind(&LedController::navCallback, this, std::placeholders::_1)); + estop_sub_ = this->create_subscription( + estop_topic_, 10, std::bind(&LedController::estopCallback, this, std::placeholders::_1)); + timer_ = this->create_wall_timer(500ms, std::bind(&LedController::sendSerialCommand, this)); + + if (serial_port_.isOpen()) { + serial_port_.write("ON\n"); + } +} + +LedController::~LedController() +{ + if (serial_port_.isOpen()) { + serial_port_.write("OFF\n"); + serial_port_.close(); + } +} + +void LedController::navCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + nav_state_ = msg->data; +} + +void LedController::estopCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + estop_state_ = msg->data; +} + +void LedController::sendSerialCommand() +{ + led_state_ = (nav_state_ && !estop_state_) ? "FLASH" : "SOLID"; + + if (serial_port_.isOpen()) { + try { + serial_port_.write(led_state_ + "\n"); + // RCLCPP_INFO(this->get_logger(), "Sent: %s", led_state_.c_str()); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to write to serial port: %s", e.what()); + } + } else { + RCLCPP_ERROR(this->get_logger(), "Serial port is NOT open!"); + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/orange_sensor_tools/src/wheel_imu_odom.cpp b/orange_sensor_tools/src/wheel_imu_odom.cpp new file mode 100644 index 0000000..e290ab9 --- /dev/null +++ b/orange_sensor_tools/src/wheel_imu_odom.cpp @@ -0,0 +1,148 @@ +#include "wheel_imu_odom.hpp" + +OdomFusionNode::OdomFusionNode() : Node("wheel_imu_odom") +{ + imu_topic_ = this->declare_parameter("imu_topic", "/imu"); + odom_topic_ = this->declare_parameter("odom_topic", "/odom"); + fused_odom_topic_ = this->declare_parameter("fused_odom_topic", "/odom/wheel_imu"); + + odom_header_frame_ = this->declare_parameter("odom_header_frame", "odom"); + odom_child_frame_ = this->declare_parameter("odom_child_frame", "base_footprint"); + TF_header_frame_ = this->declare_parameter("TF_header_frame", "odom"); + TF_child_frame_ = this->declare_parameter("TF_child_frame", "base_footprint"); + + scale_factor_ = this->declare_parameter("scale_factor", 0.45); + pitch_diff_th_ = this->declare_parameter("pitch_difference_threshold", 0.1); + publish_odom_ = this->declare_parameter("publish_odom", true); + publish_TF_ = this->declare_parameter("publish_TF", true); + debug_ = this->declare_parameter("debug", false); + + imu_sub_ = this->create_subscription( + imu_topic_, 1, std::bind(&OdomFusionNode::imuCallback, this, std::placeholders::_1)); + odom_sub_ = this->create_subscription( + odom_topic_, 1, std::bind(&OdomFusionNode::odomCallback, this, std::placeholders::_1)); + fused_odom_pub_ = this->create_publisher(fused_odom_topic_, 1); + + tf_broadcaster_ = std::make_shared(this); + + roll_ = 0.0; + pitch_ = 0.0; + yaw_ = 0.0; + position_x_ = 0.0; + position_y_ = 0.0; + position_z_ = 0.0; + + imu_received_ = false; + baseline_initialized_ = false; + baseline_pitch_ = 0.0; + prev_time_ = this->now(); + + RCLCPP_INFO(this->get_logger(), "Initialized wheel_imu_odom_node."); + RCLCPP_INFO(this->get_logger(), "publish_odom: %s", publish_odom_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "publish_TF: %s", publish_TF_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "debug: %s", debug_ ? "true" : "false"); +} + +void OdomFusionNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + orientation_x_ = msg->orientation.x; + orientation_y_ = msg->orientation.y; + orientation_z_ = msg->orientation.z; + orientation_w_ = msg->orientation.w; + + ang_vel_x_ = msg->angular_velocity.x; + ang_vel_y_ = msg->angular_velocity.y; + ang_vel_z_ = msg->angular_velocity.z; + + getYawFromQuaternion(msg->orientation, roll_, pitch_, yaw_); + imu_received_ = true; +} + +void OdomFusionNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + if (!imu_received_) return; + + rclcpp::Time current_time = msg->header.stamp; + double dt = (current_time - prev_time_).seconds(); + prev_time_ = current_time; + + // Calculate position + double dx = msg->twist.twist.linear.x * dt * scale_factor_; // best: 0.45 + position_x_ += dx * cos(yaw_); + position_y_ += dx * sin(yaw_); + + // Store initial value of pitch + if (!baseline_initialized_) { + baseline_pitch_ = pitch_; + baseline_initialized_ = true; + RCLCPP_INFO(this->get_logger(), "Baseline pitch initialized: %f", baseline_pitch_); + } + + // Determine position.z update by pitch difference + double delta_pitch = pitch_ - baseline_pitch_; + if (delta_pitch < -pitch_diff_th_) { + position_z_ -= dx * sin(delta_pitch); + } else if (delta_pitch > pitch_diff_th_) { + position_z_ -= dx * sin(delta_pitch); + } + + // Construct TF + if (publish_TF_) { + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = TF_header_frame_; + t.child_frame_id = TF_child_frame_; + t.transform.translation.x = position_x_; + t.transform.translation.y = position_y_; + t.transform.translation.z = position_z_; + t.transform.rotation.x = orientation_x_; + t.transform.rotation.y = orientation_y_; + t.transform.rotation.z = orientation_z_; + t.transform.rotation.w = orientation_w_; + tf_broadcaster_->sendTransform(t); + } + + // Construct Odom message + if (publish_odom_) { + nav_msgs::msg::Odometry fused_msg; + fused_msg.header.stamp = this->get_clock()->now(); + fused_msg.header.frame_id = odom_header_frame_; + fused_msg.child_frame_id = odom_child_frame_; + fused_msg.pose.pose.position.x = position_x_; + fused_msg.pose.pose.position.y = position_y_; + fused_msg.pose.pose.position.z = position_z_; + fused_msg.pose.pose.orientation.x = orientation_x_; + fused_msg.pose.pose.orientation.y = orientation_y_; + fused_msg.pose.pose.orientation.z = orientation_z_; + fused_msg.pose.pose.orientation.w = orientation_w_; + fused_msg.twist.twist.linear.x = msg->twist.twist.linear.x; + fused_msg.twist.twist.linear.y = 0.0; + fused_msg.twist.twist.angular.x = ang_vel_x_; + fused_msg.twist.twist.angular.y = ang_vel_y_; + fused_msg.twist.twist.angular.z = ang_vel_z_; + fused_odom_pub_->publish(fused_msg); + } + + // Debugging Feature + if (debug_) { + RCLCPP_INFO( + this->get_logger(), "x: %f, y: %f, z: %f, pitch: %f, yaw: %f", position_x_, position_y_, + position_z_, pitch_, yaw_); + } +} + +void OdomFusionNode::getYawFromQuaternion( + const geometry_msgs::msg::Quaternion & q, double & roll, double & pitch, double & yaw) +{ + tf2::Quaternion tf_q; + tf2::fromMsg(q, tf_q); + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}