From 1bf0c37ef9d93649905ab0d9bc1bca103f9a0529 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 30 Jul 2025 04:04:52 +0000 Subject: [PATCH 1/9] Update rosinstall file. --- orange_ros2.rosinstall | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/orange_ros2.rosinstall b/orange_ros2.rosinstall index 9d570c5..733613e 100644 --- a/orange_ros2.rosinstall +++ b/orange_ros2.rosinstall @@ -26,19 +26,7 @@ local-name: linefit_ground_segmentation_ros2 uri: https://github.com/KBKN-Autonomous-Robotics-Lab/linefit_ground_segmentation_ros2.git version: main -- git: - local-name: FAST_LIO - uri: https://github.com/KBKN-Autonomous-Robotics-Lab/FAST_LIO.git - version: main -- git: - local-name: fast_odom_convert - uri: https://github.com/KBKN-Autonomous-Robotics-Lab/fast_odom_convert.git - version: main - git: local-name: pcd_convert uri: https://github.com/KBKN-Autonomous-Robotics-Lab/pcd_convert.git version: main -- git: - local-name: livox_laser_simulation_RO2 - uri: https://github.com/stm32f303ret6/livox_laser_simulation_RO2.git - version: main From 9f7cddf98849b4905c242f546955a97a26e320e6 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 09:13:16 +0000 Subject: [PATCH 2/9] Add led_node. --- orange_ros2.rosinstall | 48 +++++++++++++ orange_sensor_tools/firmware/LED.ino | 70 +++++++++++++++++++ orange_sensor_tools/include/led_node.hpp | 36 ++++++++++ orange_sensor_tools/src/led_node.cpp | 85 ++++++++++++++++++++++++ 4 files changed, 239 insertions(+) create mode 100644 orange_sensor_tools/firmware/LED.ino create mode 100644 orange_sensor_tools/include/led_node.hpp create mode 100644 orange_sensor_tools/src/led_node.cpp diff --git a/orange_ros2.rosinstall b/orange_ros2.rosinstall index 9d570c5..9783939 100644 --- a/orange_ros2.rosinstall +++ b/orange_ros2.rosinstall @@ -42,3 +42,51 @@ local-name: livox_laser_simulation_RO2 uri: https://github.com/stm32f303ret6/livox_laser_simulation_RO2.git version: main +- git: + local-name: velodyne + uri: https://github.com/ros-drivers/velodyne.git + version: ros2 +- git: + local-name: icm_20948 + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/icm_20948.git + version: humble-devel +- git: + local-name: estop_ros + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/estop_ros.git + version: humble-devel +- git: + local-name: orange_navigation + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/orange_navigation.git + version: humble-devel +- git: + local-name: kbkn_maps + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/kbkn_maps.git + version: main +- git: + local-name: multi_map_manager + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/multi_map_manager.git + version: humble-devel +- git: + local-name: linefit_ground_segmentation_ros2 + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/linefit_ground_segmentation_ros2.git + version: main +- git: + local-name: FAST_LIO + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/FAST_LIO.git + version: main +- git: + local-name: fast_odom_convert + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/fast_odom_convert.git + version: main +- git: + local-name: pcd_convert + uri: https://github.com/KBKN-Autonomous-Robotics-Lab/pcd_convert.git + version: main +- git: + 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/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..e92b63e --- /dev/null +++ b/orange_sensor_tools/include/led_node.hpp @@ -0,0 +1,36 @@ +#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/src/led_node.cpp b/orange_sensor_tools/src/led_node.cpp new file mode 100644 index 0000000..11ff8aa --- /dev/null +++ b/orange_sensor_tools/src/led_node.cpp @@ -0,0 +1,85 @@ +#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; +} From a5d20a749bc535b64be8b378e88aba8fd4b45df9 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 09:14:18 +0000 Subject: [PATCH 3/9] Add wheel_imu_odom. --- orange_sensor_tools/CMakeLists.txt | 35 +++- .../include/wheel_imu_odom.hpp | 67 ++++++++ orange_sensor_tools/package.xml | 5 + orange_sensor_tools/src/wheel_imu_odom.cpp | 155 ++++++++++++++++++ 4 files changed, 260 insertions(+), 2 deletions(-) create mode 100644 orange_sensor_tools/include/wheel_imu_odom.hpp create mode 100644 orange_sensor_tools/src/wheel_imu_odom.cpp 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/include/wheel_imu_odom.hpp b/orange_sensor_tools/include/wheel_imu_odom.hpp new file mode 100644 index 0000000..3cf83e3 --- /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/wheel_imu_odom.cpp b/orange_sensor_tools/src/wheel_imu_odom.cpp new file mode 100644 index 0000000..1460665 --- /dev/null +++ b/orange_sensor_tools/src/wheel_imu_odom.cpp @@ -0,0 +1,155 @@ +#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; +} From fde160bc9044050083725f7ac5f0939903f6edb3 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 09:30:01 +0000 Subject: [PATCH 4/9] apply format --- orange_sensor_tools/include/led_node.hpp | 47 ++++---- .../include/wheel_imu_odom.hpp | 14 +-- orange_sensor_tools/src/led_node.cpp | 100 ++++++++---------- orange_sensor_tools/src/wheel_imu_odom.cpp | 33 +++--- 4 files changed, 88 insertions(+), 106 deletions(-) diff --git a/orange_sensor_tools/include/led_node.hpp b/orange_sensor_tools/include/led_node.hpp index e92b63e..28e9a31 100644 --- a/orange_sensor_tools/include/led_node.hpp +++ b/orange_sensor_tools/include/led_node.hpp @@ -1,36 +1,37 @@ #ifndef LED_NODE_HPP #define LED_NODE_HPP -#include -#include #include + #include +#include +#include class LedController : public rclcpp::Node { public: - LedController(); - ~LedController(); + 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_; + 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 +#endif // LED_NODE_HPP diff --git a/orange_sensor_tools/include/wheel_imu_odom.hpp b/orange_sensor_tools/include/wheel_imu_odom.hpp index 3cf83e3..bc391ca 100644 --- a/orange_sensor_tools/include/wheel_imu_odom.hpp +++ b/orange_sensor_tools/include/wheel_imu_odom.hpp @@ -1,13 +1,14 @@ #ifndef WHEEL_IMU_ODOM_HPP #define WHEEL_IMU_ODOM_HPP +#include +#include + +#include +#include #include #include -#include -#include -#include #include -#include class OdomFusionNode : public rclcpp::Node { @@ -18,8 +19,7 @@ class OdomFusionNode : public rclcpp::Node 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); + const geometry_msgs::msg::Quaternion & q, double & roll, double & pitch, double & yaw); rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr odom_sub_; @@ -64,4 +64,4 @@ class OdomFusionNode : public rclcpp::Node double ang_vel_z_; }; -#endif // WHEEL_IMU_ODOM_HPP +#endif // WHEEL_IMU_ODOM_HPP diff --git a/orange_sensor_tools/src/led_node.cpp b/orange_sensor_tools/src/led_node.cpp index 11ff8aa..be1bc0a 100644 --- a/orange_sensor_tools/src/led_node.cpp +++ b/orange_sensor_tools/src/led_node.cpp @@ -2,84 +2,72 @@ using namespace std::chrono_literals; -LedController::LedController() - : Node("LED_node"), nav_state_(false), estop_state_(false) +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"); + 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()); - } + 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)); + 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"); - } + if (serial_port_.isOpen()) { + serial_port_.write("ON\n"); + } } LedController::~LedController() { - if (serial_port_.isOpen()) - { - serial_port_.write("OFF\n"); - serial_port_.close(); - } + 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; + nav_state_ = msg->data; } void LedController::estopCallback(const std_msgs::msg::Bool::SharedPtr msg) { - estop_state_ = msg->data; + 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!"); + 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[]) +int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + 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 index 1460665..e290ab9 100644 --- a/orange_sensor_tools/src/wheel_imu_odom.cpp +++ b/orange_sensor_tools/src/wheel_imu_odom.cpp @@ -5,12 +5,12 @@ 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); @@ -72,8 +72,7 @@ void OdomFusionNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) position_y_ += dx * sin(yaw_); // Store initial value of pitch - if (!baseline_initialized_) - { + if (!baseline_initialized_) { baseline_pitch_ = pitch_; baseline_initialized_ = true; RCLCPP_INFO(this->get_logger(), "Baseline pitch initialized: %f", baseline_pitch_); @@ -81,18 +80,14 @@ void OdomFusionNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) // Determine position.z update by pitch difference double delta_pitch = pitch_ - baseline_pitch_; - if (delta_pitch < -pitch_diff_th_) - { + if (delta_pitch < -pitch_diff_th_) { position_z_ -= dx * sin(delta_pitch); - } - else if (delta_pitch > pitch_diff_th_) - { + } else if (delta_pitch > pitch_diff_th_) { position_z_ -= dx * sin(delta_pitch); } // Construct TF - if (publish_TF_) - { + if (publish_TF_) { geometry_msgs::msg::TransformStamped t; t.header.stamp = this->get_clock()->now(); t.header.frame_id = TF_header_frame_; @@ -108,8 +103,7 @@ void OdomFusionNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) } // Construct Odom message - if (publish_odom_) - { + 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_; @@ -130,23 +124,22 @@ void OdomFusionNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr 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_); + 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) + 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) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); From 88f667ba2fa53d9bd7c5269d13dd85d7ad03fccb Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 10:22:26 +0000 Subject: [PATCH 5/9] Delete livox and replace with velodyne. --- .../launch/data_processing.launch.xml | 31 ++++++------------- orange_bringup/launch/orange_robot.launch.xml | 14 ++++----- 2 files changed, 16 insertions(+), 29 deletions(-) diff --git a/orange_bringup/launch/data_processing.launch.xml b/orange_bringup/launch/data_processing.launch.xml index abef442..31a414f 100644 --- a/orange_bringup/launch/data_processing.launch.xml +++ b/orange_bringup/launch/data_processing.launch.xml @@ -7,8 +7,6 @@ - - @@ -33,35 +31,24 @@ - - - - - - - + + + - - - - - - + + diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 624a90d..32c4dea 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -62,7 +62,7 @@ - - + + @@ -94,7 +94,7 @@ --> - + - + From 4eca6d6c7cbf2eddbe0d22d88b69ddb9baf9d0c9 Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 11:08:22 +0000 Subject: [PATCH 6/9] Migrate GNSS-related programs to orange_gnss. --- orange_bringup/orange_bringup/get_lonlat.py | 152 ------ orange_bringup/setup.py | 10 +- .../orange_gnss}/GPSodom_correction.py | 0 orange_gnss/orange_gnss/__init__.py | 0 .../orange_gnss}/combination.py | 2 +- .../orange_gnss}/ekf_myself.py | 111 +++-- .../orange_gnss}/ekf_myself_noGPS.py | 0 .../orange_gnss}/fix_to_GPSodom.py | 0 orange_gnss/orange_gnss/get_lonlat_ttyACM.py | 104 ++++ orange_gnss/orange_gnss/get_lonlat_ttyUSB.py | 134 +++++ .../orange_gnss/get_movingbase_quat_ttyUSB.py | 142 ++++++ ...ss_odom_movingbase_fix_publisher_ttyUSB.py | 460 ++++++++++++++++++ .../orange_gnss/gnss_odom_publisher_ttyUSB.py | 391 +++++++++++++++ .../orange_gnss}/lonlat_to_odom.py | 3 +- .../orange_gnss}/movingbase_yaw_to_quat.py | 9 +- orange_gnss/package.xml | 23 + orange_gnss/resource/orange_gnss | 0 orange_gnss/setup.cfg | 4 + orange_gnss/setup.py | 42 ++ 19 files changed, 1387 insertions(+), 200 deletions(-) delete mode 100755 orange_bringup/orange_bringup/get_lonlat.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/GPSodom_correction.py (100%) create mode 100644 orange_gnss/orange_gnss/__init__.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/combination.py (97%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/ekf_myself.py (80%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/ekf_myself_noGPS.py (100%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/fix_to_GPSodom.py (100%) create mode 100755 orange_gnss/orange_gnss/get_lonlat_ttyACM.py create mode 100644 orange_gnss/orange_gnss/get_lonlat_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py create mode 100644 orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/lonlat_to_odom.py (98%) rename {orange_bringup/orange_bringup => orange_gnss/orange_gnss}/movingbase_yaw_to_quat.py (94%) create mode 100644 orange_gnss/package.xml create mode 100644 orange_gnss/resource/orange_gnss create mode 100644 orange_gnss/setup.cfg create mode 100644 orange_gnss/setup.py diff --git a/orange_bringup/orange_bringup/get_lonlat.py b/orange_bringup/orange_bringup/get_lonlat.py deleted file mode 100755 index 1aa8417..0000000 --- a/orange_bringup/orange_bringup/get_lonlat.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python3 -import math - -import rclpy -import serial -from nav_msgs.msg import Odometry -from rclpy.node import Node -from sensor_msgs.msg import NavSatFix, NavSatStatus -from std_msgs.msg import Header - - -class GPSData(Node): - def __init__(self): - super().__init__('gps_data_acquisition') - - self.declare_parameter('port', '/dev/sensors/GNSSbase') - self.declare_parameter('baud', 9600) - self.declare_parameter('country_id', 0) - self.declare_parameter('type', 1) # 1=ttyACM 2=ttyUSB - - self.dev_name = self.get_parameter( - 'port').get_parameter_value().string_value - self.serial_baud = self.get_parameter( - 'baud').get_parameter_value().integer_value - self.country_id = self.get_parameter( - 'country_id').get_parameter_value().integer_value - self.type = self.get_parameter( - 'type').get_parameter_value().integer_value - - self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) - self.lonlat_msg = NavSatFix() - - self.initial_coordinate = None - self.fix_data = None - - self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) - - self.get_logger().info("Start get_lonlat node") - self.get_logger().info("---------------------") - - def get_gps(self, dev_name, country_id): - try: - serial_port = serial.Serial(dev_name, self.serial_baud) - except serial.SerialException as serialerror: - self.get_logger().error(f"Serial error: {serialerror}") - return None - - initial_letters = None - if country_id == 0: # Japan - initial_letters = "$GNGGA" - elif country_id == 1: # USA - initial_letters = "$GPGGA" - - if self.type == 1: - while True: - line = serial_port.readline().decode('latin-1') - gps_data = line.split(',') - if gps_data[0] == initial_letters: - break - - Fixtype_data = int(gps_data[6]) - if Fixtype_data != 0: - satelitecount_data = float(gps_data[7]) - # ddmm.mmmmm to dd.ddddd - latitude_data = float(gps_data[2]) / 100.0 - if gps_data[3] == 'S': # south - latitude_data *= -1 - # ddmm.mmmmm to dd.ddddd - longitude_data = float(gps_data[4]) / 100.0 - if gps_data[5] == 'W': # west - longitude_data *= -1 - altitude_data = float(gps_data[9]) - else: - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - - elif self.type == 2: - line = serial_port.readline() - talker_ID = line.find(initial_letters) - if talker_ID != -1: - line = line[(talker_ID-1):] - gps_data = line.split(b",") - rospy.loginfo(gps_data) - Fixtype_data = int(gps_data[6]) - rospy.loginfo(Fixtype_data) - if Fixtype_data != 0: - satelitecount_data = int(gps_data[7]) - rospy.loginfo(satelitecount_data) - if Fixtype_data != 0: - # ddmm.mmmmm to dd.ddddd - latitude_data = float(gps_data[2]) / 100.0 - if gps_data[3] == b"S": # south - latitude_data *= -1 - # ddmm.mmmmm to dd.ddddd - longitude_data = float(gps_data[4]) / 100.0 - if gps_data[5] == b"W": # west - longitude_data *= -1 - altitude_data = float(gps_data[9]) - else: - # not fix data - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - else: - # no GPS data - latitude_data = 0 - longitude_data = 0 - altitude_data = 0 - satelitecount_data = 0 - else: - rospy.loginfo( - "current latitude and longitude (Fixtype,latitude, longitude,altitude):None") - return None - - serial_port.close() - gnggadata = (Fixtype_data, latitude_data, longitude_data, - altitude_data, satelitecount_data) - - return gnggadata - - def publish_GPS_lonlat(self): - lonlat = self.get_gps(self.dev_name, self.country_id) - if lonlat: - self.lonlat_msg.header = Header() - self.lonlat_msg.header.frame_id = "gps" - self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() - - self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ - 0] != 0 else NavSatStatus.STATUS_NO_FIX - self.lonlat_msg.latitude = float(lonlat[1]) - self.lonlat_msg.longitude = float(lonlat[2]) - self.lonlat_msg.altitude = float(lonlat[3]) - - self.lonlat_pub.publish(self.lonlat_msg) - # self.get_logger().info(f"Published GPS data: {lonlat}") - else: - self.get_logger().error("!!! -gps data error- !!!") - - -def main(args=None): - rclpy.init(args=args) - gpslonlat = GPSData() - rclpy.spin(gpslonlat) - gpslonlat.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/orange_bringup/setup.py b/orange_bringup/setup.py index 1e3ca3c..10c00b8 100644 --- a/orange_bringup/setup.py +++ b/orange_bringup/setup.py @@ -29,15 +29,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "motor_driver_node = orange_bringup.motor_driver_node:main", - "fix_to_GPSodom = orange_bringup.fix_to_GPSodom:main", - "movingbase_yaw_to_quat = orange_bringup.movingbase_yaw_to_quat:main", - "combination = orange_bringup.combination:main", - "ekf_myself = orange_bringup.ekf_myself:main", - "get_lonlat = orange_bringup.get_lonlat:main", - "GPSodom_correction = orange_bringup.GPSodom_correction:main", - "lonlat_to_odom = orange_bringup.lonlat_to_odom:main", - "ekf_myself_noGPS = orange_bringup.ekf_myself_noGPS:main" + "motor_driver_node = orange_bringup.motor_driver_node:main" ], }, ) diff --git a/orange_bringup/orange_bringup/GPSodom_correction.py b/orange_gnss/orange_gnss/GPSodom_correction.py similarity index 100% rename from orange_bringup/orange_bringup/GPSodom_correction.py rename to orange_gnss/orange_gnss/GPSodom_correction.py diff --git a/orange_gnss/orange_gnss/__init__.py b/orange_gnss/orange_gnss/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/orange_bringup/orange_bringup/combination.py b/orange_gnss/orange_gnss/combination.py similarity index 97% rename from orange_bringup/orange_bringup/combination.py rename to orange_gnss/orange_gnss/combination.py index f8e8e21..1cd3900 100644 --- a/orange_bringup/orange_bringup/combination.py +++ b/orange_gnss/orange_gnss/combination.py @@ -28,7 +28,7 @@ def __init__(self): self.timer = self.create_timer(1.0, self.publish_combined_odom) self.get_logger().info("Start combination node") - self.get_logger().info("----------------------") + self.get_logger().info("-------------------------") def odomgps_callback(self, msg): self.x = msg.pose.pose.position.x diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_gnss/orange_gnss/ekf_myself.py similarity index 80% rename from orange_bringup/orange_bringup/ekf_myself.py rename to orange_gnss/orange_gnss/ekf_myself.py index 8da39ef..50d3b99 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_gnss/orange_gnss/ekf_myself.py @@ -43,13 +43,20 @@ def __init__(self): self.robot_orientationz = 0 self.robot_orientationw = 0 self.Number_of_satellites = 0 + + ###### angle offset ####### + self.GPS_angle_conut = 0 + self.GPS_angle_offset = 0 + self.sub_a = self.create_subscription( - Odometry, '/odom_fast', self.sensor_a_callback, 10) + Odometry, '/odom', self.sensor_a_callback, 10) self.sub_b = self.create_subscription( - Odometry, '/odom_CLAS_movingbase', self.sensor_b_callback, 10) + Odometry, '/odom/UM982', self.sensor_b_callback, 10) + #self.sub_b = self.create_subscription( + # Odometry, '/odom_ref_slam', self.sensor_b_callback, 10) - self.declare_parameter("ekf_publish_TF", True) + self.declare_parameter("ekf_publish_TF", False) self.ekf_publish_TF = self.get_parameter( "ekf_publish_TF").get_parameter_value().bool_value @@ -62,7 +69,7 @@ def __init__(self): self.timer = self.create_timer(0.1, self.publish_fused_value) self.get_logger().info("Start ekf_myself node") - self.get_logger().info("---------------------") + self.get_logger().info("-------------------------") def orientation_to_yaw(self, z, w): yaw = np.arctan2(2.0 * (w * z), 1.0 - 2.0 * (z ** 2)) @@ -83,17 +90,21 @@ def sensor_a_callback(self, data): self.SmpTime = current_time - self.prev_time else: self.SmpTime = 0.1 - self.prev_time = current_time + self.prev_time = current_time + + current_pos = np.array([ data.pose.pose.position.x, data.pose.pose.position.y ]) + if self.prev_pos is not None: distance = np.linalg.norm(current_pos - self.prev_pos) self.Speed = distance / self.SmpTime else: self.Speed = 0 + self.prev_pos = current_pos self.GTheta = self.orientation_to_yaw( @@ -102,6 +113,12 @@ def sensor_a_callback(self, data): def sensor_b_callback(self, data): self.GpsXY = np.array( [data.pose.pose.position.x, data.pose.pose.position.y]) + #self.GpsXY = np.array( + # [data.pose.pose.position.x, data.pose.pose.position.y, 0]) + #pointcloud, rot_matrix = rotation_xyz(self.GpsXY, 0, 0, 90) + #self.GpsXY = np.array( + # [pointcloud[0], pointcloud[1]]) + self.GPStheta = self.orientation_to_yaw( data.pose.pose.orientation.z, data.pose.pose.orientation.w) @@ -111,23 +128,27 @@ def sensor_b_callback(self, data): self.GPSthetayaw0 = self.GPStheta self.Number_of_satellites = data.pose.covariance[0] # + + + + + # self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") def determination_of_R(self): if 0 <= self.Number_of_satellites < 4: # Bad - self.R1 = 1e-2 # 0.01 FAST-LIO - self.R2 = 9e-2 # 0.09 CLAS-movingbase + self.R1 = 0.17**2 # FAST-LIO + self.R2 = 0.17**2 # CLAS-movingbase self.R3 = 9 # GTheta self.R4 = 1 # GPStheta - - elif 4 <= self.Number_of_satellites < 8: # So-so - self.R1 = 6e-2 # 0.06 FAST-LIO - self.R2 = 4e-2 # 0.04 CLAS-movingbase + if 4 <= self.Number_of_satellites < 8: # So-so... + self.R1 = 0.08**2 # FAST-LIO + self.R2 = 0.08**2 # CLAS-movingbase self.R3 = 4 # GTheta self.R4 = 6 # GPStheta elif self.Number_of_satellites >= 8: # Good!!! - self.R1 = 9e-2 # 0.09 FAST-LIO - self.R2 = 1e-2 # 0.01 CLAS-movingbase + self.R1 = 0.05**2 # FAST-LIO + self.R2 = 0.05**2 # CLAS-movingbase self.R3 = 2 # GTheta self.R4 = 8 # GPStheta @@ -245,37 +266,44 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2): return theta_sum def calculate_offset(self, combyaw, GTheta, GPStheta): - deference = abs(GTheta) + abs(GPStheta) + abs_GTheta = abs(GTheta) + abs_GPStheta = abs(GPStheta) + abs_combyaw = abs(combyaw) + pi = math.pi + + deference = abs_GTheta + abs_GPStheta if GTheta > 0 and GPStheta < 0 and combyaw > 0: self.GOffset = -(GTheta - combyaw) elif GTheta > 0 and GPStheta < 0 and combyaw < 0: - self.GOffset = -(GTheta + abs(combyaw)) + self.GOffset = -(GTheta + abs_combyaw) elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta: - self.GOffset = -(abs(combyaw) - abs(GTheta)) + self.GOffset = -(abs_combyaw - abs_GTheta) elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta: - self.GOffset = -(GTheta + abs(combyaw)) + self.GOffset = -(GTheta + abs_combyaw) elif GTheta < 0 and GPStheta > 0 and combyaw > 0: - self.GOffset = abs(GTheta) + combyaw + self.GOffset = abs_GTheta + combyaw elif GTheta < 0 and GPStheta > 0 and combyaw < 0: - self.GOffset = abs(GTheta) - abs(combyaw) + self.GOffset = abs_GTheta - abs_combyaw elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta: self.GOffset = combyaw - GTheta elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > math.pi: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi: self.GOffset = combyaw - GTheta - elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > math.pi: - self.GOffset = math.pi - GTheta + math.pi - abs(combyaw) - elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > math.pi: - self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta))) - elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > math.pi: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - - if abs(self.GOffset) > 5 * math.pi / 180: # not -0.0872 ~ 0.0872 + elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi: + self.GOffset = pi - GTheta + pi - abs_combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi: + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi: + self.GOffset = -(abs_combyaw - abs_GTheta) + + if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") + # self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): @@ -285,12 +313,11 @@ def publish_fused_value(self): self.R2 = R[1] self.R3 = R[2] self.R4 = R[3] - - if self.GpsXY is not None: + if self.GpsXY is not None : fused_value = self.KalfGPSXY( self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) self.GPS_conut += 1 - if self.GPS_conut % 10 == 0: + if self.GPS_conut % 20 == 0: self.combyaw = self.combine_yaw( self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) self.offsetyaw = self.calculate_offset( @@ -354,6 +381,26 @@ def publish_fused_value(self): self.t.transform.rotation.w = float(self.robot_orientationw) self.br.sendTransform(self.t) +def rotation_xyz(pointcloud, theta_x, theta_y, theta_z): + rad_x = math.radians(theta_x) + rad_y = math.radians(theta_y) + rad_z = math.radians(theta_z) + rot_x = np.array([[ 1, 0, 0], + [ 0, math.cos(rad_x), -math.sin(rad_x)], + [ 0, math.sin(rad_x), math.cos(rad_x)]]) + + rot_y = np.array([[ math.cos(rad_y), 0, math.sin(rad_y)], + [ 0, 1, 0], + [-math.sin(rad_y), 0, math.cos(rad_y)]]) + + rot_z = np.array([[ math.cos(rad_z), -math.sin(rad_z), 0], + [ math.sin(rad_z), math.cos(rad_z), 0], + [ 0, 0, 1]]) + rot_matrix = rot_z.dot(rot_y.dot(rot_x)) + #print(f"rot_matrix ={rot_matrix}") + #print(f"pointcloud ={pointcloud.shape}") + rot_pointcloud = rot_matrix.dot(pointcloud) + return rot_pointcloud, rot_matrix def main(args=None): rclpy.init(args=args) diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_gnss/orange_gnss/ekf_myself_noGPS.py similarity index 100% rename from orange_bringup/orange_bringup/ekf_myself_noGPS.py rename to orange_gnss/orange_gnss/ekf_myself_noGPS.py diff --git a/orange_bringup/orange_bringup/fix_to_GPSodom.py b/orange_gnss/orange_gnss/fix_to_GPSodom.py similarity index 100% rename from orange_bringup/orange_bringup/fix_to_GPSodom.py rename to orange_gnss/orange_gnss/fix_to_GPSodom.py diff --git a/orange_gnss/orange_gnss/get_lonlat_ttyACM.py b/orange_gnss/orange_gnss/get_lonlat_ttyACM.py new file mode 100755 index 0000000..dd72e9f --- /dev/null +++ b/orange_gnss/orange_gnss/get_lonlat_ttyACM.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header + + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSSbase') + self.declare_parameter('baud', 9600) + self.declare_parameter('country_id', 0) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + self.country_id = self.get_parameter( + 'country_id').get_parameter_value().integer_value + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initial_coordinate = None + self.fix_data = None + + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) + + self.get_logger().info("Start get_lonlat node") + self.get_logger().info("-------------------------") + + def get_gps(self, dev_name, country_id): + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + initial_letters = None + if country_id == 0: # Japan + initial_letters = "$GNGGA" + elif country_id == 1: # USA + initial_letters = "$GPGGA" + + while True: + line = serial_port.readline().decode('latin-1') + gps_data = line.split(',') + if gps_data[0] == initial_letters: + break + + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = float(gps_data[7]) + # ddmm.mmmmm to dd.ddddd + latitude_data = float(gps_data[2]) / 100.0 + if gps_data[3] == 'S': # south + latitude_data *= -1 + # ddmm.mmmmm to dd.ddddd + longitude_data = float(gps_data[4]) / 100.0 + if gps_data[5] == 'W': # west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else: + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + + serial_port.close() + gnggadata = (Fixtype_data, latitude_data, longitude_data,altitude_data, satelitecount_data) + + return gnggadata + + def publish_GPS_lonlat(self): + lonlat = self.get_gps(self.dev_name, self.country_id) + if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + self.lonlat_msg.latitude = float(lonlat[1]) + self.lonlat_msg.longitude = float(lonlat[2]) + self.lonlat_msg.altitude = float(lonlat[3]) + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + else: + self.get_logger().error("!!!!-gps data error-!!!!") + + +def main(args=None): + rclpy.init(args=args) + gpslonlat = GPSData() + rclpy.spin(gpslonlat) + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py b/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py new file mode 100644 index 0000000..4004cbe --- /dev/null +++ b/orange_gnss/orange_gnss/get_lonlat_ttyUSB.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header + + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + self.country_id = self.get_parameter( + 'country_id').get_parameter_value().integer_value + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initial_coordinate = None + self.fix_data = None + + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat) + + self.get_logger().info("Start get_lonlat node") + self.get_logger().info("-------------------------") + + def get_gps(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + else: + self.get_logger().error("!--not GPS data--!") + return None + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data) + return gnggadata + + + def publish_GPS_lonlat(self): + lonlat = self.get_gps(self.dev_name, self.country_id) + if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + self.lonlat_msg.latitude = float(lonlat[1]) + self.lonlat_msg.longitude = float(lonlat[2]) + self.lonlat_msg.altitude = float(lonlat[3]) + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + else: + self.get_logger().error("!!!!-gps data error-!!!!") + + +def main(args=None): + rclpy.init(args=args) + gpslonlat = GPSData() + rclpy.spin(gpslonlat) + gpslonlat.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py b/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py new file mode 100644 index 0000000..ca65b5b --- /dev/null +++ b/orange_gnss/orange_gnss/get_movingbase_quat_ttyUSB.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +import math +import rclpy +import serial +from rclpy.node import Node +from sensor_msgs.msg import Imu +from std_msgs.msg import Header + +class GPS_heading_Data(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + + self.dev_name = self.get_parameter( + 'port').get_parameter_value().string_value + self.serial_baud = self.get_parameter( + 'baud').get_parameter_value().integer_value + + self.heading_pub = self.create_publisher(Imu, 'movingbase/quat', 10) + self.movingbase_msg = Imu() + + self.timer = self.create_timer(1.0, self.movingbase_publish_msg) + self.count = 0 + + self.get_logger().info("Start get_movingbase_quat_ttyUSB node") + self.get_logger().info("-------------------------") + + def get_gps_heading(self, dev_name): + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + + serial_port.close() + + return heading + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + def movingbase_publish_msg(self): + real_heading = self.get_gps_heading(self.dev_name) + #self.get_logger().info(f"real_heading: {real_heading}") + if real_heading is not None and real_heading != 0: + + robotheading = real_heading + 90 + if robotheading >= 360: + robotheading -= 360 + + #self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = q[1] + self.movingbase_msg.orientation.y = q[2] + self.movingbase_msg.orientation.z = -q[3] # -z + self.movingbase_msg.orientation.w = q[0] + self.movingbase_msg.orientation_covariance[0] = robotheading + self.heading_pub.publish(self.movingbase_msg) + else: + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = 0.0 + self.movingbase_msg.orientation.y = 0.0 + self.movingbase_msg.orientation.z = 0.0 + self.movingbase_msg.orientation.w = 0.0 + self.heading_pub.publish(self.movingbase_msg) + self.get_logger().error("!!!!-not movingbase data-!!!!") + +def main(args=None): + rclpy.init(args=args) + GPS_heading_Data_node = GPS_heading_Data() + rclpy.spin(GPS_heading_Data_node) + GPS_heading_Data_node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py new file mode 100644 index 0000000..9f8810c --- /dev/null +++ b/orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py @@ -0,0 +1,460 @@ +#!/usr/bin/env python3 +import math +import rclpy +import serial +import tkinter as tk +from rclpy.node import Node +from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus +from nav_msgs.msg import Odometry +from std_msgs.msg import Header +from geometry_msgs.msg import Quaternion, Pose, Point, Twist, Vector3 +import threading +import time +from my_msgs.srv import Avglatlon + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + # Parameters + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + self.declare_parameter('Position_magnification', 1.675) + self.declare_parameter('heading', 90.0) + + self.dev_name = self.get_parameter('port').get_parameter_value().string_value + self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value + self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value + self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value + self.theta = self.get_parameter('heading').get_parameter_value().double_value + + #self.theta = 275.6 # tukuba param + #self.theta = 180 #nakaniwa param + self.initial_coordinate = None + self.fix_data = None + self.count = 0 + self.initialized = False # 平均初期座標が取得できたかどうか + + # Publishers + self.lonlat_pub = self.create_publisher(NavSatFix, '/fix', 1) + self.lonlat_msg = NavSatFix() + self.movingbase_pub = self.create_publisher(Imu, '/movingbase/quat', 10) + self.movingbase_msg = Imu() + self.odom_pub = self.create_publisher(Odometry, '/odom/UM982', 10) + self.odom_msg = Odometry() + + # service client + self.client = self.create_client(Avglatlon, 'send_avg_gps') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available...") + + # Timers + self.timer = self.create_timer(1.0, self.timer_callback) + + self.first_heading = None + + self.gps_data_cache = None + + self.get_logger().info("Start get_lonlat_movingbase_quat_ttyUSB node") + self.get_logger().info("-------------------------") + + # tkinter GUI setup + self.root = tk.Tk() + self.root.title("GPS Data Acquisition") + self.start_button = tk.Button(self.root, text="Start", command=self.start_gps_acquisition, width=20, height = 5) + self.start_button.pack() + + self.gps_acquisition_thread = None + self.is_acquiring = False + + # service client + def send_request(self): + request = Avglatlon.Request() + request.avg_lat = self.initial_coordinate[0] # ← average lat + request.avg_lon = self.initial_coordinate[1] # ← average lon + request.theta = self.theta + + future = self.client.call_async(request) + future.add_done_callback(self.response_callback) + + def response_callback(self, future): + try: + response = future.result() + if response.success: + self.get_logger().info('サービス送信成功') + else: + self.get_logger().warn('サービスは受け取られましたが、処理は失敗しました') + except Exception as e: + self.get_logger().error(f'サービス呼び出し失敗: {e}') + + # timer callback + def timer_callback(self): + if not self.initialized: + # 初期化が完了していないので何もしない + return + self.gps_data_cache = self.get_gps_quat(self.dev_name, self.country_id) + + if self.gps_data_cache: + fix_type, lat, lon, alt, _, heading = self.gps_data_cache + if fix_type != 0: + self.publish_fix(self.gps_data_cache) + self.publish_odom(lat, lon, alt) + self.publish_movingbase(heading) + else: + self.get_logger().error("GPS not fixed") + else: + self.get_logger().error("Failed to get GPS data") + + # gps data collect + def start_gps_acquisition(self): + if not self.is_acquiring: + self.is_acquiring = True + self.gps_acquisition_thread = threading.Thread(target=self.acquire_gps_data) + self.gps_acquisition_thread.start() + + def acquire_gps_data(self): + lat_sum = 0.0 + lon_sum = 0.0 + count = 0 + + start_time = time.time() + while time.time() - start_time < 10: # 10 seconds + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + lat_sum += GPS_data[1] + lon_sum += GPS_data[2] + count += 1 + time.sleep(0.1) # Slight delay to avoid overwhelming the GPS device + + if count > 0: + self.initial_coordinate = [lat_sum / count, lon_sum / count] + self.initialized = True + self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}") + self.send_request() + self.is_acquiring = False + + def get_gps_quat(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + return gnggadata + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + def heading_to_quat(self ,real_heading): + + robotheading = real_heading + 90 + if robotheading >= 360: + robotheading -= 360 + + #self.get_logger().info(f"real_heading: {real_heading}") + #self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + return q + + def conversion(self, coordinate, origin, theta): + ido = coordinate[0] + keido = coordinate[1] + ido0 = origin[0] + keido0 = origin[1] + + # self.get_logger().info(f"theta: {theta}") + + a = 6378137 + f = 35/10439 + e1 = 734/8971 + e2 = 127/1547 + n = 35/20843 + a0 = 1 + a2 = 102/40495 + a4 = 1/378280 + a6 = 1/289634371 + a8 = 1/204422462123 + pi180 = 71/4068 + # %math.pi/180 + d_ido = ido - ido0 + d_keido = keido - keido0 + rd_ido = d_ido * pi180 + rd_keido = d_keido * pi180 + r_ido = ido * pi180 + r_keido = keido * pi180 + r_ido0 = ido0 * pi180 + W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2)) + N = a / W + t = math.tan(r_ido) + ai = e2*math.cos(r_ido) + + # %===Y===% + S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) - + a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n) + S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) - + a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n) + m0 = S/S0 + B = S-S0 + y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2 + y2 = (rd_keido**4)*N*math.sin(r_ido) * \ + (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24 + y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \ + (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720 + gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3) + + # %===X===% + x1 = rd_keido*N*math.cos(r_ido) + x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6 + x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \ + (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120 + gps_x = self.Position_magnification * m0 * (x1 + x2 + x3) + + # point = (gps_x, gps_y)Not match + + degree_to_radian = math.pi / 180 + r_theta = theta * degree_to_radian + h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y + h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y + + point = (h_y, -h_x) + + return point + + def publish_fix(self, gps): + #lonlat = self.get_gps_quat(self.dev_name, self.country_id) + #if lonlat: + self.lonlat_msg.header = Header() + self.lonlat_msg.header.frame_id = "gps" + self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if gps[ + 0] != 0 else NavSatStatus.STATUS_NO_FIX + #self.lonlat_msg.latitude = float(lonlat[1]) + #self.lonlat_msg.longitude = float(lonlat[2]) + #self.lonlat_msg.altitude = float(lonlat[3]) + self.lonlat_msg.latitude = gps[1] + self.lonlat_msg.longitude = gps[2] + self.lonlat_msg.altitude = gps[3] + + self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + #else: + # self.get_logger().error("!!!!-gps data error-!!!!") + + def publish_movingbase(self, heading): + if heading is not None and heading != 0.0: + robotheading = heading + 90.0 + if robotheading >= 360.0: + robotheading -= 360.0 + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360.0 + if relative_heading > 180.0: + relative_heading -= 360.0 + + movingbaseyaw = relative_heading * (math.pi / 180.0) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + q = self.quaternion_from_euler(roll, pitch, yaw) + + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = q[1] + self.movingbase_msg.orientation.y = q[2] + self.movingbase_msg.orientation.z = -q[3] # -z方向 + self.movingbase_msg.orientation.w = q[0] + self.movingbase_msg.orientation_covariance[0] = robotheading + + self.movingbase_pub.publish(self.movingbase_msg) + + else: + self.movingbase_msg.header.stamp = self.get_clock().now().to_msg() + self.movingbase_msg.header.frame_id = "imu_link" + self.movingbase_msg.orientation.x = 0.0 + self.movingbase_msg.orientation.y = 0.0 + self.movingbase_msg.orientation.z = 0.0 + self.movingbase_msg.orientation.w = 0.0 + + self.movingbase_pub.publish(self.movingbase_msg) + self.get_logger().error("!!!!-not movingbase data-!!!!") + + + def publish_odom(self, lat, lon, alt): + + #GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + #if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + if self.gps_data_cache and self.gps_data_cache[1] != 0 and self.gps_data_cache[2] != 0: + GPS_data = self.gps_data_cache + self.satelite = GPS_data[4] + lonlat = [GPS_data[1], GPS_data[2]] + + if self.initial_coordinate is None: + self.initial_coordinate = [GPS_data[1], GPS_data[2]] + GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta) + GPSquat = self.heading_to_quat(GPS_data[5]) + + self.odom_msg.header.stamp = self.get_clock().now().to_msg() + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "base_footprint" + self.odom_msg.pose.pose.position.x = GPSxy[0] + self.odom_msg.pose.pose.position.y = GPSxy[1] + + self.odom_msg.pose.pose.orientation.x = GPSquat[1] + self.odom_msg.pose.pose.orientation.y = GPSquat[2] + self.odom_msg.pose.pose.orientation.z = -GPSquat[3] + self.odom_msg.pose.pose.orientation.w = GPSquat[0] + # Number of satellites + self.odom_msg.pose.covariance[0] = float(self.satelite) + self.odom_pub.publish(self.odom_msg) + else: + self.get_logger().error("!!!!-NOT RECIEVE-gps data error-!!!!") + +def main(args=None): + #rclpy.init(args=args) + #gpslonlat = GPSData() + #rclpy.spin(gpslonlat) + #gpslonlat.root.mainloop() + #gpslonlat.destroy_node() + #rclpy.shutdown() + rclpy.init(args=args) + gpslonlat = GPSData() + ros_thread = threading.Thread(target=rclpy.spin, args=(gpslonlat,)) + ros_thread.start() + gpslonlat.root.mainloop() # tkinter GUI表示 + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() + diff --git a/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py new file mode 100644 index 0000000..7e2973d --- /dev/null +++ b/orange_gnss/orange_gnss/gnss_odom_publisher_ttyUSB.py @@ -0,0 +1,391 @@ +#!/usr/bin/env python3 +import rclpy +import serial +import math +import tkinter as tk +from nav_msgs.msg import Odometry +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix, NavSatStatus +from std_msgs.msg import Header +import threading +import time +from my_msgs.srv import Avglatlon + +class GPSData(Node): + def __init__(self): + super().__init__('gps_data_acquisition') + + self.declare_parameter('port', '/dev/sensors/GNSS_UM982') + self.declare_parameter('baud', 115200) + self.declare_parameter('country_id', 0) + self.declare_parameter('Position_magnification', 1.675) + self.declare_parameter('heading', 180.0) + + self.dev_name = self.get_parameter('port').get_parameter_value().string_value + self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value + self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value + self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value + #self.theta = self.get_parameter('heading').get_parameter_value().double_value + + self.initial_coordinate = None + self.fix_data = None + self.count = 0 + + self.odom_pub = self.create_publisher(Odometry, "/odom/UM982", 10) + self.odom_msg = Odometry() + + self.lonlat_pub = self.create_publisher(NavSatFix, "/fix", 1) + self.lonlat_msg = NavSatFix() + + self.initialized = False # 平均初期座標が取得できたかどうか + self.timer = self.create_timer(1.0, self.publish_GPS_lonlat_quat) + + # service client + self.client = self.create_client(Avglatlon, 'send_avg_gps') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("service not available...") + + self.get_logger().info("Start get_lonlat quat node") + self.get_logger().info("-------------------------") + + # tkinter GUI setup + self.root = tk.Tk() + self.root.title("GPS Data Acquisition") + self.start_button = tk.Button(self.root, text="Start GPS Acquisition", command=self.start_gps_acquisition) + self.start_button.pack() + + self.gps_acquisition_thread = None + self.is_acquiring = False + + # service client + def send_request(self): + request = Avglatlon.Request() + request.avg_lat = self.initial_coordinate[0] # ← average lat + request.avg_lon = self.initial_coordinate[1] # ← average lon + request.theta = self.theta + + future = self.client.call_async(request) + future.add_done_callback(self.response_callback) + + def response_callback(self, future): + try: + response = future.result() + if response.success: + self.get_logger().info('サービス送信成功') + else: + self.get_logger().warn('サービスは受け取られましたが、処理は失敗しました') + except Exception as e: + self.get_logger().error(f'サービス呼び出し失敗: {e}') + + # gps data collect + def start_gps_acquisition(self): + if not self.is_acquiring: + self.is_acquiring = True + self.gps_acquisition_thread = threading.Thread(target=self.acquire_gps_data) + self.gps_acquisition_thread.start() + + def acquire_gps_data(self): + lat_sum = 0.0 + lon_sum = 0.0 + heading_sum = 0.0 + count = 0 + #serial_port = serial.Serial(self.dev_name, self.serial_baud) + #line = serial_port.readline() + + start_time = time.time() + while time.time() - start_time < 10: # 10 seconds + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gps_data = line.split(b",") + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + lat_sum += GPS_data[1] + lon_sum += GPS_data[2] + heading_sum += float(GPS_data[5]) + count += 1 + time.sleep(0.1) # Slight delay to avoid overwhelming the GPS device + + if count > 0: + self.initial_coordinate = [lat_sum / count, lon_sum / count] + self.theta = (heading_sum / count) - 90 + self.initialized = True + self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}") + self.get_logger().info(f"Initial theta set to: {self.theta}") + self.send_request() + self.is_acquiring = False + + def get_gps_quat(self, dev_name, country_id): + # interface with sensor device(as a serial port) + try: + serial_port = serial.Serial(dev_name, self.serial_baud) + except serial.SerialException as serialerror: + self.get_logger().error(f"Serial error: {serialerror}") + return None + + # country info + if country_id == 0: # Japan + initial_letters = b"GNGGA" + elif country_id == 1: # USA + initial_letters = b"GPGGA" + else: # not certain + initial_letters = None + + initial_letters_outdoor = b"$GNHDT" + initial_letters_indoor = b"$GPHDT" + + while(1): + line = serial_port.readline() + #self.get_logger().info(f"line: {line}") + talker_ID_indoor = line.find(initial_letters_indoor) + talker_ID_outdoor = line.find(initial_letters_outdoor) + if talker_ID_indoor != -1: + #self.get_logger().info("GPHDT ok") + #line = line[(talker_ID_indoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + if talker_ID_outdoor != -1: + #self.get_logger().info("GNHDT ok") + #line = line[(talker_ID_outdoor-1):] + gps_data = line.split(b",") + #self.get_logger().info(f"gps_data: {gps_data}") + heading = float(gps_data[1]) + if heading is None: + self.get_logger().error("not GPS heading data") + heading = 0 + break + +# gps_data = ["$G?GGA", +# "UTC time", +# "Latitude (ddmm.mmmmm)", +# "latitude type (south/north)", +# "Longitude (ddmm.mmmmm)", +# "longitude type (east longitude/west longitude)", +# "Fixtype", +# "Number of satellites used for positioning", +# "HDOP", +# "Altitude", +# "M(meter)", +# "Elevation", +# "M(meter)", +# "", +# "checksum"] + + line = serial_port.readline() + talker_ID = line.find(initial_letters) + if talker_ID != -1: + line = line[(talker_ID-1):] + gps_data = line.split(b",") + Fixtype_data = int(gps_data[6]) + if Fixtype_data != 0: + satelitecount_data = int(gps_data[7])### + if Fixtype_data != 0: + latitude_data = float(gps_data[2]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[3] == b"S":#south + latitude_data *= -1 + longitude_data = float(gps_data[4]) / 100.0 # ddmm.mmmmm to dd.ddddd + if gps_data[5] == b"W":#west + longitude_data *= -1 + altitude_data = float(gps_data[9]) + else : + #not fix data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not fix data--!") + else : + #no GPS data + Fixtype_data = 0 + latitude_data = 0 + longitude_data = 0 + altitude_data = 0 + satelitecount_data = 0 + self.get_logger().error("!--not GPS data--!") + + serial_port.close() + + gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + return gnggadata + + def quaternion_from_euler(self, roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + return q + + + def heading_to_quat(self ,real_heading): + + robotheading = real_heading - 90 + if robotheading >= 360: + robotheading -= 360 + + self.get_logger().info(f"real_heading: {real_heading}") + self.get_logger().info(f"robotheading: {robotheading}") + + if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!") + self.first_heading = robotheading + self.count = 1 + + relative_heading = robotheading - self.first_heading + if relative_heading < 0: + relative_heading += 360 + + if relative_heading > 180: + relative_heading -= 360 + + movingbaseyaw = relative_heading * (math.pi / 180) + + roll, pitch = 0.0, 0.0 + yaw = movingbaseyaw + + q = self.quaternion_from_euler(roll, pitch, yaw) + # self.get_logger().info(f"Quaternion: {q}") + + return q + + def conversion(self, coordinate, origin, theta): + ido = coordinate[0] + keido = coordinate[1] + ido0 = origin[0] + keido0 = origin[1] + + # self.get_logger().info(f"theta: {theta}") + + a = 6378137 + f = 35/10439 + e1 = 734/8971 + e2 = 127/1547 + n = 35/20843 + a0 = 1 + a2 = 102/40495 + a4 = 1/378280 + a6 = 1/289634371 + a8 = 1/204422462123 + pi180 = 71/4068 + # %math.pi/180 + d_ido = ido - ido0 + d_keido = keido - keido0 + rd_ido = d_ido * pi180 + rd_keido = d_keido * pi180 + r_ido = ido * pi180 + r_keido = keido * pi180 + r_ido0 = ido0 * pi180 + W = math.sqrt(1-(e1**2)*(math.sin(r_ido)**2)) + N = a / W + t = math.tan(r_ido) + ai = e2*math.cos(r_ido) + + # %===Y===% + S = a*(a0*r_ido - a2*math.sin(2*r_ido)+a4*math.sin(4*r_ido) - + a6*math.sin(6*r_ido)+a8*math.sin(8*r_ido))/(1+n) + S0 = a*(a0*r_ido0-a2*math.sin(2*r_ido0)+a4*math.sin(4*r_ido0) - + a6*math.sin(6*r_ido0)+a8*math.sin(8*r_ido0))/(1+n) + m0 = S/S0 + B = S-S0 + y1 = (rd_keido**2)*N*math.sin(r_ido)*math.cos(r_ido)/2 + y2 = (rd_keido**4)*N*math.sin(r_ido) * \ + (math.cos(r_ido)**3)*(5-(t**2)+9*(ai**2)+4*(ai**4))/24 + y3 = (rd_keido**6)*N*math.sin(r_ido)*(math.cos(r_ido)**5) * \ + (61-58*(t**2)+(t**4)+270*(ai**2)-330*(ai**2)*(t**2))/720 + gps_y = self.Position_magnification * m0 * (B + y1 + y2 + y3) + + # %===X===% + x1 = rd_keido*N*math.cos(r_ido) + x2 = (rd_keido**3)*N*(math.cos(r_ido)**3)*(1-(t**2)+(ai**2))/6 + x3 = (rd_keido**5)*N*(math.cos(r_ido)**5) * \ + (5-18*(t**2)+(t**4)+14*(ai**2)-58*(ai**2)*(t**2))/120 + gps_x = self.Position_magnification * m0 * (x1 + x2 + x3) + + # point = (gps_x, gps_y)Not match + + degree_to_radian = math.pi / 180 + r_theta = theta * degree_to_radian + h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y + h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y + #point = (-h_y, h_x) + point = (h_y, -h_x) + + return point + + def publish_GPS_lonlat_quat(self): + if not self.initialized: + # 初期化が完了していないので何もしない + return + + GPS_data = self.get_gps_quat(self.dev_name, self.country_id) + #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading) + if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0: + self.satelite = GPS_data[4] + lonlat = [GPS_data[1], GPS_data[2]] + + # publish fix topic + #self.lonlat_msg.header = Header() + #self.lonlat_msg.frame_id = "gps" + #self.lonlat_msg.header.stamp = self.get_clock().now().to_msg() + + #self.lonlat_msg.status.status = NavSatStatus.STATUS_FIX if lonlat[ + # 0] != 0 else NavSatStatus.STATUS_NO_FIX + #self.lonlat_msg.latitude = GPS_data[1] # ido + #self.lonlat_msg.longitude = GPS_data[2] # keido + #self.lonlat_msg.altitude = GPS_data[3] # koudo + + #self.lonlat_pub.publish(self.lonlat_msg) + # self.get_logger().info(f"Published GPS data: {lonlat}") + + if self.initial_coordinate is None: + self.initial_coordinate = [GPS_data[1], GPS_data[2]] + GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta) + GPSquat = self.heading_to_quat(GPS_data[5]) + + self.odom_msg.header.stamp = self.get_clock().now().to_msg() + self.odom_msg.header.frame_id = "odom" + self.odom_msg.child_frame_id = "base_footprint" + self.odom_msg.pose.pose.position.x = GPSxy[0] + self.odom_msg.pose.pose.position.y = GPSxy[1] + + self.odom_msg.pose.pose.orientation.x = GPSquat[1] + self.odom_msg.pose.pose.orientation.y = GPSquat[2] + self.odom_msg.pose.pose.orientation.z = -GPSquat[3] + self.odom_msg.pose.pose.orientation.w = GPSquat[0] + # Number of satellites + self.odom_msg.pose.covariance[0] = float(self.satelite) + self.odom_pub.publish(self.odom_msg) + else: + self.get_logger().error("!!!!-NOT RECIEVE-gps data error-!!!!") + + +def main(args=None): + #rclpy.init(args=args) + #gpslonlat = GPSData() + #rclpy.spin(gpslonlat) + #gpslonlat.root.mainloop() + #gpslonlat.destroy_node() + #rclpy.shutdown() + rclpy.init(args=args) + gpslonlat = GPSData() + + ros_thread = threading.Thread(target=rclpy.spin, args=(gpslonlat,)) + ros_thread.start() + + gpslonlat.root.mainloop() # tkinter GUI表示 + + gpslonlat.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/orange_bringup/orange_bringup/lonlat_to_odom.py b/orange_gnss/orange_gnss/lonlat_to_odom.py similarity index 98% rename from orange_bringup/orange_bringup/lonlat_to_odom.py rename to orange_gnss/orange_gnss/lonlat_to_odom.py index 63685aa..c604822 100755 --- a/orange_bringup/orange_bringup/lonlat_to_odom.py +++ b/orange_gnss/orange_gnss/lonlat_to_odom.py @@ -111,8 +111,7 @@ def conversion(self, coordinate, origin, theta): r_theta = theta * degree_to_radian h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y - point = (h_y, -h_x) - # point = (-h_y, h_x) + point = (-h_y, h_x) # point = (h_y, -h_x) return point diff --git a/orange_bringup/orange_bringup/movingbase_yaw_to_quat.py b/orange_gnss/orange_gnss/movingbase_yaw_to_quat.py similarity index 94% rename from orange_bringup/orange_bringup/movingbase_yaw_to_quat.py rename to orange_gnss/orange_gnss/movingbase_yaw_to_quat.py index adea144..b781fd7 100644 --- a/orange_bringup/orange_bringup/movingbase_yaw_to_quat.py +++ b/orange_gnss/orange_gnss/movingbase_yaw_to_quat.py @@ -32,7 +32,7 @@ def __init__(self): self.movingbase_data = None self.get_logger().info("Start movingbase_yaw_to_quat node") - self.get_logger().info("---------------------------------") + self.get_logger().info("-------------------------") # def movingbase_callback(self, data): # self.movingbase_data = data @@ -80,7 +80,7 @@ def checksum(self, ackPacket, payloadlength): return True else: self.get_logger().error("ACK Checksum Failure") - self.get_logger().error("!!! -movingbase receive error- !!!") + self.get_logger().error("!!!!-movingbase receive error-!!!!") return False def parse_heading(self, ackPacket): @@ -138,9 +138,10 @@ def movingbase_publish_msg(self): if heading >= 360: heading -= 360 - self.get_logger().info(f"robotheading: {heading}") + #self.get_logger().info(f"robotheading: {heading}") if self.count == 0: + self.get_logger().info(f"!!!----------robotheading: {heading} deg----------!!!") self.first_heading = heading self.count = 1 @@ -170,7 +171,7 @@ def movingbase_publish_msg(self): self.heading_pub.publish(self.movingbase_msg) self.movingbase_data = None else: - self.get_logger().error("!!! -movingbase data error- !!!") + self.get_logger().error("!!!!-movingbase data error-!!!!") def main(args=None): diff --git a/orange_gnss/package.xml b/orange_gnss/package.xml new file mode 100644 index 0000000..637ce96 --- /dev/null +++ b/orange_gnss/package.xml @@ -0,0 +1,23 @@ + + + + orange_gnss + 0.0.0 + For GNSS + Saito + Apache-2.0 + orange_description + rviz2 + rclpy + std_msgs + geometry_msgs + nav_msgs + python3-pymodbus + velodyne + urg_node + icm_20948 + joint_state_publisher + + ament_python + + diff --git a/orange_gnss/resource/orange_gnss b/orange_gnss/resource/orange_gnss new file mode 100644 index 0000000..e69de29 diff --git a/orange_gnss/setup.cfg b/orange_gnss/setup.cfg new file mode 100644 index 0000000..fa25766 --- /dev/null +++ b/orange_gnss/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/orange_gnss +[install] +install_scripts=$base/lib/orange_gnss diff --git a/orange_gnss/setup.py b/orange_gnss/setup.py new file mode 100644 index 0000000..a675aff --- /dev/null +++ b/orange_gnss/setup.py @@ -0,0 +1,42 @@ +import os +from glob import glob + +from setuptools import setup + +package_name = "orange_gnss" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", + ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer=["Saito"], + maintainer_email=[ + "shun.saito.6t@stu.hosei.ac.jp", + ], + description="For GNSS", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "GPSodom_correction = orange_gnss.GPSodom_correction:main", + "combination = orange_gnss.combination:main", + "ekf_myself = orange_gnss.ekf_myself:main", + "ekf_myself_noGPS = orange_gnss.ekf_myself_noGPS:main", + "fix_to_GPSodom = orange_gnss.fix_to_GPSodom:main", + "get_lonlat_ttyACM = orange_gnss.get_lonlat_ttyACM:main", + "get_lonlat_ttyUSB = orange_gnss.get_lonlat_ttyUSB:main", + "get_movingbase_quat_ttyUSB = orange_gnss.get_movingbase_quat_ttyUSB:main", + "gnss_odom_movingbase_fix_publisher_ttyUSB = orange_gnss.gnss_odom_movingbase_fix_publisher_ttyUSB:main", + "gnss_odom_publisher_ttyUSB = orange_gnss.gnss_odom_publisher_ttyUSB:main", + "lonlat_to_odom = orange_gnss.lonlat_to_odom:main", + "movingbase_yaw_to_quat = orange_gnss.movingbase_yaw_to_quat:main" + ], + }, +) From aed0b292acede30fafc2597c8e06e4bacf93d37f Mon Sep 17 00:00:00 2001 From: shunsugar Date: Wed, 6 Aug 2025 11:08:51 +0000 Subject: [PATCH 7/9] Update launch files. --- .../launch/data_processing.launch.xml | 13 +++---- orange_bringup/launch/orange_robot.launch.xml | 16 ++------- orange_bringup/launch/with_ros2bag.launch.xml | 34 +++---------------- 3 files changed, 13 insertions(+), 50 deletions(-) diff --git a/orange_bringup/launch/data_processing.launch.xml b/orange_bringup/launch/data_processing.launch.xml index 31a414f..0a28aa3 100644 --- a/orange_bringup/launch/data_processing.launch.xml +++ b/orange_bringup/launch/data_processing.launch.xml @@ -16,17 +16,14 @@ --> - - - - + - + - + @@ -44,11 +41,11 @@ - + diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 32c4dea..1a77510 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -62,29 +62,19 @@ - + - - - - - + - + - - - - - - - - + - + - + - - - - - - - - - - - - - - -