From e73ef2a5a802ccebe8eccebdb5eff135315b86c1 Mon Sep 17 00:00:00 2001
From: ray <2954701669@qq.com>
Date: Thu, 14 Mar 2024 08:48:40 +0000
Subject: [PATCH] ultrasonic

---
 .../ros_ws/src/upbot_vision/CMakeLists.txt    | 13 ++++--
 .../include/upbot_vision/ultrasonic.h         | 24 ++++++++++
 .../ros_ws/src/upbot_vision/src/ultrasonic.cc | 46 +++++++++++++++++++
 3 files changed, 79 insertions(+), 4 deletions(-)
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h
 create mode 100644 Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc

diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt
index f39ac6e..95f81b0 100644
--- a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(OpenCV  REQUIRED)
 find_package(Threads REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread chrono)
 
 
 # generate_messages(
@@ -41,10 +42,13 @@ add_library(
   include/upbot_vision/preprocess.h
   include/upbot_vision/ranging.h
   include/upbot_vision/Timer.h
+  include/upbot_vision/ultrasonic.h
+
   src/detection.cc
   src/postprocess.cc
   src/preprocess.cc
   src/ranging.cc
+  src/ultrasonic.cc
 )
 
 add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -53,13 +57,14 @@ target_link_libraries(head
  
 )
 add_executable(main src/main.cc)
-# add_executable(sub_dis src/sub_dis.cc)
+add_executable(ultrasonic src/ultrasonic.cc)
 add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS}  ${catkin_EXPORTED_TARGETS})
 # add_dependencies(sub_dis ${${PROJECT_NAME}_EXPORTED_TARGETS} upbot_vision_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+target_link_libraries(ultrasonic 
+  ${Boost_LIBRARIES} Boost::thread Boost::chrono
+  ${catkin_LIBRARIES}
+  )
 
-# target_link_libraries(sub_dis 
-# ${catkin_LIBRARIES}
-# )
 target_link_libraries(main
 head
 ${catkin_LIBRARIES}
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h
new file mode 100644
index 0000000..6ff8f5d
--- /dev/null
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/include/upbot_vision/ultrasonic.h
@@ -0,0 +1,24 @@
+#include <ros/ros.h>
+#include <sensor_msgs/Range.h>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+
+
+class UltrasonicNode {
+private:
+    ros::NodeHandle nh;
+    ros::Publisher ultrasonic_pub;
+    boost::asio::io_service io;
+    boost::asio::serial_port serial;
+
+public:
+    UltrasonicNode() : nh("~"), serial(io), ultrasonic_pub(nh.advertise<sensor_msgs::Range>("ultrasonic", 1)) {
+        std::string port_name;
+        nh.param<std::string>("port", port_name, "/dev/ttyUSB0");
+        serial.open(port_name);
+        serial.set_option(boost::asio::serial_port_base::baud_rate(9600));
+    }
+    void readUltrasonic();
+};
+ 
\ No newline at end of file
diff --git a/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc
new file mode 100644
index 0000000..02e946b
--- /dev/null
+++ b/Code/MowingRobot/pibot_ros/ros_ws/src/upbot_vision/src/ultrasonic.cc
@@ -0,0 +1,46 @@
+#include "upbot_vision/ultrasonic.h"
+
+
+void UltrasonicNode::readUltrasonic() {
+    try {
+        while (true) {
+            // 发送数据
+            serial.write_some(boost::asio::buffer("\xA0", 1));
+            // ROS_INFO("Data sent: 0xA0");
+
+            // 读取数据
+            char data[3];
+            boost::asio::read(serial, boost::asio::buffer(data, 3));
+            sensor_msgs::Range range_msg;
+            range_msg.header.stamp = ros::Time::now();
+            range_msg.header.frame_id = "ultrasonic_sensor";
+            range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
+            range_msg.field_of_view = M_PI; // Assuming 180 degrees
+            range_msg.min_range = 0.0; // Minimum range in meters
+            range_msg.max_range = 5.0; // Maximum range in meters
+            range_msg.range = ((data[0] << 16) + (data[1] << 8 ) + data[2]) / 1000.0; // Convert to meters
+            ultrasonic_pub.publish(range_msg);
+            ROS_INFO("Received ultrasonic data: %f mm", range_msg.range);
+
+            // 延迟一段时间
+            boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
+        }
+    } catch (std::exception& e) {
+        ROS_ERROR("Error reading ultrasonic data: %s", e.what());
+    }
+}
+
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "ultrasonic_node");
+    UltrasonicNode ultrasonic_node;
+    ros::Rate rate(10); // 10 Hz
+
+    while (ros::ok()) {
+        ultrasonic_node.readUltrasonic();
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    return 0;
+}