Cx330_HomeWork/ROS_Learn
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

初步完成ROS服务程序工作

Browse Source
This commit is contained in:
张梦南 2025-07-02 14:38:38 +08:00
parent 4d34c200dc
commit b645464b0b
6 changed files with 333 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/Service/CMakeLists.txt Normal file
View File

@ -0,0 +1,24 @@
cmake_minimum_required(VERSION 3.0.2)
project(Service)
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
geometry_msgs
visualization_msgs
message_generation
)
add_service_files(
FILES
FFT.srv
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs geometry_msgs visualization_msgs
)

79
src/Service/package.xml Normal file
View File

@ -0,0 +1,79 @@
<?xml version="1.0"?>
<package format="2">
<name>Service</name>
<version>0.0.0</version>
<description>The Service package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="root@todo.todo">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/Service</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

27
src/Service/scripts/Publisher_S.py Normal file
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python3
import rospy
import math
from geometry_msgs.msg import PointStamped
def main():
rospy.init_node('Publisher_S')
pub = rospy.Publisher('position', PointStamped, queue_size=10) # 向 position 话题发布 PointStamped 类型的消息
rate = rospy.Rate(10) # 每 0.1s 一条信息
k = 0
while not rospy.is_shutdown():
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map" # 基于 map 的坐标系
msg.point.x = math.sin(0.05 * math.pi * k) # x 值
msg.point.y = math.cos(0.05 * math.pi * k) # y 值
msg.point.z = 0.0
pub.publish(msg) # 发布话题
rospy.loginfo("发布端发送坐标信息: x=%.3f, y=%.3f" % (msg.point.x, msg.point.y)) # 打印坐标信息
k += 1
rate.sleep()
if __name__ == '__main__':
main()

48
src/Service/scripts/Service_S.py Normal file
View File

@ -0,0 +1,48 @@
#!/usr/bin/env python3
import rospy
import math, cmath
from Service.srv import FFT, FFTResponse
def computeFFT(input_data):
N = len(input_data)
magnitude = [0.0] * (N//2 + 1) # 存储幅值
phase = [0.0] * (N//2 + 1) # 存储相位
for k in range(N//2 + 1):
s = 0+0j
for n in range(N):
angle = -2 * math.pi * k * n / N # 角度计算
s += input_data[n] * complex(math.cos(angle), math.sin(angle)) # 快速傅里叶变换公式
magnitude[k] = abs(s) # 幅值
phase[k] = cmath.phase(s) # 相位
return magnitude, phase
def fft_callback(req):
if len(req.data_x) != req.window_size or len(req.data_y) != req.window_size:
rospy.logerr("数据量不符合128点FFT要求")
return FFTResponse()
mag_x, phase_x = computeFFT(req.data_x) # 对坐标 x 和 y 分别进行快速傅里叶变换FFT
mag_y, phase_y = computeFFT(req.data_y)
res = FFTResponse()
res.magnitude_x = mag_x # 坐标 x 的幅度
res.magnitude_y = mag_y # 坐标 y 的幅度
res.phase_x = phase_x # 坐标 x 的相位
res.phase_y = phase_y # 坐标 y 的相位
rospy.loginfo("FFT 服务完成 %d 个点" % req.window_size)
return res
if __name__ == '__main__':
rospy.init_node('Service_S')
service = rospy.Service('fft_service', FFT, fft_callback) # ROS 服务
rospy.loginfo("服务端快速傅里叶变换程序执行成功")
rospy.spin()

147
src/Service/scripts/Subscriber_Client_S.py Normal file
View File

@ -0,0 +1,147 @@
#!/usr/bin/env python3
import rospy
import random
from geometry_msgs.msg import PointStamped, Point
from visualization_msgs.msg import Marker, MarkerArray
from Service.srv import FFT
MAX_POINTS = 100
FFT_WINDOW_SIZE = 128
original_trajectory = []
noisy_trajectory = []
noisy_x_history = []
noisy_y_history = []
current_x_magnitudes = []
current_y_magnitudes = []
noisy_pub = None
marker_pub = None
fft_client = None
def position_callback(msg):
global original_trajectory, noisy_trajectory
global noisy_x_history, noisy_y_history
global current_x_magnitudes, current_y_magnitudes
global noisy_pub, marker_pub, fft_client
noise_x = random.uniform(-0.1, 0.1) # 产生 -0.1 ～0.1 的随机噪声
noise_y = random.uniform(-0.1, 0.1)
noisy_msg = PointStamped()
noisy_msg.header = msg.header
noisy_msg.point.x = msg.point.x + noise_x # 在原信号上叠加噪声
noisy_msg.point.y = msg.point.y + noise_y
noisy_msg.point.z = 0.0
original_trajectory.append(msg.point) # 原点轨迹
if len(original_trajectory) > MAX_POINTS:
original_trajectory.pop(0)
noisy_trajectory.append(noisy_msg.point) #加噪轨迹
if len(noisy_trajectory) > MAX_POINTS:
noisy_trajectory.pop(0)
noisy_x_history.append(noisy_msg.point.x) #保存历史坐标信息
noisy_y_history.append(noisy_msg.point.y)
marker_array = MarkerArray()
# 原始信号轨迹绘制
original_line = Marker()
original_line.header = msg.header
original_line.ns = "trajectory"
original_line.id = 0
original_line.type = Marker.LINE_STRIP
original_line.action = Marker.ADD
original_line.scale.x = 0.02
original_line.color.g = 1.0 # 绿色线
original_line.color.a = 1.0
original_line.points = list(original_trajectory)
marker_array.markers.append(original_line)
# 加噪信号轨迹绘制
noisy_line = Marker()
noisy_line.header = msg.header
noisy_line.ns = "trajectory"
noisy_line.id = 1
noisy_line.type = Marker.LINE_STRIP
noisy_line.action = Marker.ADD
noisy_line.scale.x = 0.02
noisy_line.color.r = 1.0 # 红色线
noisy_line.color.a = 1.0
noisy_line.points = list(noisy_trajectory)
marker_array.markers.append(noisy_line)
# 原始信号位置点
original_point = Marker()
original_point.header = msg.header
original_point.ns = "current_point"
original_point.id = 0
original_point.type = Marker.SPHERE
original_point.action = Marker.ADD
original_point.pose.position = msg.point
original_point.pose.orientation.w = 1.0
original_point.scale.x = 0.1
original_point.scale.y = 0.1
original_point.scale.z = 0.1
original_point.color.g = 1.0 # 绿色球
original_point.color.a = 1.0
marker_array.markers.append(original_point)
# 加噪信号位置点
noisy_point = Marker()
noisy_point.header = msg.header
noisy_point.ns = "current_point"
noisy_point.id = 1
noisy_point.type = Marker.SPHERE
noisy_point.action = Marker.ADD
noisy_point.pose.position = noisy_msg.point
noisy_point.pose.orientation.w = 1.0
noisy_point.scale.x = 0.1
noisy_point.scale.y = 0.1
noisy_point.scale.z = 0.1
noisy_point.color.r = 1.0 # 红色球
noisy_point.color.a = 1.0
marker_array.markers.append(noisy_point)
# 当点数 >=128 时利用快速傅里叶变换FFT计算加噪信号频谱
if len(noisy_x_history) >= FFT_WINDOW_SIZE:
try:
res = fft_client(data_x=noisy_x_history[-FFT_WINDOW_SIZE:],
data_y=noisy_y_history[-FFT_WINDOW_SIZE:],
window_size=FFT_WINDOW_SIZE)
rospy.loginfo("FFT computed successfully")
current_x_magnitudes[:] = res.magnitude_x
current_y_magnitudes[:] = res.magnitude_y
except rospy.ServiceException as e:
rospy.logerr("Failed to call FFT service: %s" % e)
# 判断频谱，生成频谱图
if current_x_magnitudes and current_y_magnitudes:
x_spectrum = Marker()
x_spectrum.header = msg.header
x_spectrum.ns = "坐标x频谱"
x_spectrum.id = 2
x_spectrum.type = Marker.CUBE_LIST
x_spectrum.action = Marker.ADD
x_spectrum.scale.x = 0.08
x_spectrum.scale.y = 0.01
x_spectrum.scale.z = 1.0
x_spectrum.color.b = 1.0
x_spectrum.color.a = 0.8
y_spectrum = Marker()
y_spectrum.header = msg.header
y_spectrum.ns = "坐标y频谱"
y_spectrum.id = 3
y_spectrum.type = Marker.CUBE_LIST
y_spectrum.action = Marker.ADD
y_spectrum.scale.x = 0.08
y_spectrum.scale.y = 0.01
y_spectrum.scale.z = 1.0
y_spectrum.color.r = 1.0
y_spectrum.color.a = 0.8
for i in range(len(current_x_magnitudes)):
px = Point()
px.x = i * 0.1

8
src/Service/srv/FFT.srv Normal file
View File

@ -0,0 +1,8 @@
float64[] data_x
float64[] data_y
int32 window_size
---
float64[] magnitude_x
float64[] magnitude_y
float64[] phase_x
float64[] phase_y