#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import argparse
import tf
import threading
import rospy
import sys
import math
from sensor_msgs.msg import LaserScan
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from std_msgs.msg import Int32
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import roslib
import tf2_ros
from geometry_msgs.msg import TransformStamped
import numpy as np
from std_msgs.msg import String, Float32MultiArray
import ctypes
from ctypes.util import find_library
import os
from playsound import playsound
from ar_track_alvar_msgs.msg import AlvarMarkers
#from robot_voice.msg import Targettt # 自定义消息类型
key = 0
F_list = []
global xy
global task_xy
global qtn_list_xy
global qtn_list_task_xy
global pose_num_xy
global pose_num_task_xy
global yaw
global now_pose_X
global now_pose_Y
#xy =[[0.16,-0.30,3.14], [3.0,-0.32,0], [3.03,-3.02,-0], [0.01, -2.95, 3.14],[0.5, -2.55, 3.14]]
xy = [[0.21,-0.81,3.12],[0.82,-0.28,1.54], [0.821,-0.80,1.491], [2.95,-2.44,0.02],[2.37,-3.15,-1.574]]
#task_xy =[[0.17,-1.22,3.14],[1.08,-0.25,3.14],[2.17,-0.27,0], [2.93,-1.14,0], [2.96,-2.15,0],[2.10,-3.04,0],[1.09,-3.0,3.14],[0.23,-2.10,3.14]]
task_xy=[[0.821,-0.80,1.491],[2.55,-0.17,1.54], [2.99,-0.85,0.05], [2.95,-2.44,0.02],[2.37,-3.15,-1.574]]
pid_stop_xy =[[0.395,-1.06,0],[0.416,-0.856,-90],[-0.99,-0.826,90], [0.52,-0.49,180], [0.52,0.50,180],[-0.54,-0.49,180],[-0.50,0.49,0],[0.51,-0.49,0]]
qtn_list_xy = [] #四元数列表
qtn_list_task_xy = [] #任务点四元数列表
pose_num_xy= len(xy)
pose_num_task_xy=len(task_xy)
yaw = 0
global move_base
now_pose_X=0
now_pose_Y=0
# -----------pid函数及其参数------------
# 注：
# 1.
global w_kp
global w_ki
global w_kd
global w_target
global w_e_all
global w_last_e
w_kp = 2 #2
w_ki = 0.001
w_kd = 0.005 #0
w_e_all = 0
w_last_e = 0
global x_f,x_b,y_l,y_r
x_f=0.0
x_b=0.0
y_l=0.0
y_r=0.0
def w_pid_cal(pid_target,dis):
global w_kp
global w_ki
global w_kd
global w_e_all
global w_last_e
e = dis -pid_target
#if e>-0.1 and e<0.1:
#e = 0
w_e_all = w_e_all+e
pid = w_kp*e+w_ki*w_e_all+w_kd*(e-w_last_e)
w_last_e = e
return pid
global p_kp
global p_ki
global p_kd
global p_e_all
global p_last_e
global p_pid
p_kp = -7
p_ki = 0
p_kd = -3
p_e_all = 0
p_last_e = 0
p_pid = 0
def p_pid_cal(pid_target,pose):
global p_kp
global p_ki
global p_kd
global p_e_all
global p_last_e
ture_pose = (pose/3.14159265359*180.0+180.0)%360
if pid_target==0:
if ture_pose>0 and ture_pose<180:
pid_target=0
if ture_pose>180 and ture_pose<360:
pid_target=360
e = ture_pose -pid_target
# print(e)
p_e_all = p_e_all+e
pid = p_kp*e+p_ki*p_e_all+p_kd*(e-p_last_e)
#rospy.loginfo("e %f",e)
p_last_e = e
return pid
global point_kp
global point_ki
global point_kd
global point_e_all
global point_last_e
global point_pid
point_kp = -3
point_ki = 0
point_kd = 0
point_e_all = 0
point_last_e = 0
point_pid = 0
def point_pid(pid_target_x,ture):
global point_kp
global point_ki
global point_kd
global point_e_all
global point_last_e
e = ture -pid_target_x
point_e_all = point_e_all+e
pid = point_kp*e+point_ki*point_e_all+point_kd*(e-point_last_e)
point_last_e = e
return pid
def limt(limt,target):
if limt>target:
limt=target
if limt<-target:
limt=-target
return limt
# ----------pid停车-------
def pid_stop2(target_x,target_y,target_yaw):
global w_kp,w_ki,w_kd,w_e_all
w_kp = 1.5 #1.5
w_ki = 0.005 #0.005
w_kd = 0.006 #0.01
w_e_all=0
count =0
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7)
speed = Twist()
wich_x = 0
wich_y = 0
time = 0
while not rospy.is_shutdown():
rate_loop_pid.sleep()
time+=1
if target_x>0:
pid_x = w_pid_cal(target_x,x_f)
wich_x=x_f
if target_x<0:
pid_x = w_pid_cal(target_x,-x_b)
wich_x=-x_b
if target_y>0:
pid_y = w_pid_cal(target_y,y_l)
wich_y = y_l
if target_y<0:
pid_y = w_pid_cal(target_y,-y_r)
wich_y = -y_r
p_pid = p_pid_cal(target_yaw,yaw)
#if abs(wich_x)<0.8:
#speed.linear.y = 0
#else:
speed.linear.y = pid_y
speed.linear.x = pid_x
speed.angular.z = p_pid/180.0*3.14159265359
w_e_all=limt(w_e_all,5)
print("w_e_all:",w_e_all)
print("wich_x:",wich_x,"wich_y:",wich_y)
if time>=150:
w_e_all=0
break
#if abs(wich_x-target_x)<=0.05 and abs(wich_y-target_y)<=0.07 and abs(target_yaw-(yaw/3.1415926*180+180))<=5:
#w_e_all=5
#w_ki = 0.001
if abs(wich_x-target_x)<=0.03 and abs(wich_y-target_y)<=0.03 and abs(target_yaw-(yaw/3.1415926*180+180))<=3:
w_e_all=0
count+=1
if count>=6:
speed.linear.x = 0
speed.linear.y = 0
speed.linear.z = 0
pid_vel_pub.publish(speed)
#rospy.sleep(0.5)
w_e_all=0
break
pid_vel_pub.publish(speed)
def pid_stop(target_x,target_y,target_yaw):
global w_kp,w_ki,w_kd,w_e_all
w_kp = 1.5 #1.5
w_ki = 0.005 #0.005
w_kd = 0.006 #0.01
w_e_all=0
count =0
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7)
speed = Twist()
wich_x = 0
wich_y = 0
time = 0
while not rospy.is_shutdown():
rate_loop_pid.sleep()
time+=1
if target_x>0:
pid_x = w_pid_cal(target_x,x_f)
wich_x=x_f
if target_x<0:
pid_x = w_pid_cal(target_x,-x_b)
wich_x=-x_b
if target_y>0:
pid_y = w_pid_cal(target_y,y_l)
wich_y = y_l
if target_y<0:
pid_y = w_pid_cal(target_y,-y_r)
wich_y = -y_r
p_pid = p_pid_cal(target_yaw,yaw)
#if abs(wich_x)<0.8:
#speed.linear.y = 0
#else:
speed.linear.y = pid_y
speed.linear.x = pid_x
speed.angular.z = p_pid/180.0*3.14159265359
w_e_all=limt(w_e_all,5)
print("w_e_all:",w_e_all)
print("wich_x:",wich_x,"wich_y:",wich_y)
if time>=150:
w_e_all=0
break
#if abs(wich_x-target_x)<=0.05 and abs(wich_y-target_y)<=0.07 and abs(target_yaw-(yaw/3.1415926*180+180))<=5:
#w_e_all=5
#w_ki = 0.001
if abs(wich_x-target_x)<=0.08 and abs(wich_y-target_y)<=0.08 and abs(target_yaw-(yaw/3.1415926*180+180))<=5:
w_e_all=0
count+=1
if count>=3:
speed.linear.x = 0
speed.linear.y = 0
speed.linear.z = 0
pid_vel_pub.publish(speed)
#rospy.sleep(0.5)
w_e_all=0
break
pid_vel_pub.publish(speed)
def pid_go(target_x,target_y,target_yaw):
global point_kp, vision_result, dis_trun_off,point_ki
global w_kp,w_ki,w_kd
w_kp = 2 #2
w_ki = 0
w_kd = 0.008#0
count =0
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7)
speed = Twist()
wich_x = 0
wich_y = 0
while not rospy.is_shutdown():
if target_x>0:
pid_x = w_pid_cal(target_x,x_f)
wich_x=x_f
if target_x<0:
pid_x = w_pid_cal(target_x,-x_b)
wich_x=-x_b
if target_y>0:
pid_y = w_pid_cal(target_y,y_l)
wich_y = y_l
if target_y<0:
pid_y = w_pid_cal(target_y,-y_r)
wich_y = -y_r
p_pid = p_pid_cal(target_yaw,yaw)
if abs(target_x-wich_x)<0.2 and abs(target_y-wich_y)<0.2:
speed.linear.x = 0
speed.linear.y = 0
else:
speed.linear.y = pid_y
speed.linear.x = pid_x
speed.angular.z = p_pid/180.0*3.14159265359
pid_vel_pub.publish(speed)
rate_loop_pid.sleep()
print("x_f:",x_f,"y_l:",y_l)
#print(abs(target_yaw-yaw/3.1415926*180))
if vision_result != 0:
# rospy.sleep(0.3)
# thread_dis.join()
break
def pid_go2(target_x,target_y,target_yaw):
global vision_result
global w_kp,w_ki,w_kd,w_e_all
print("--------开始pid_go2--------")
w_kp = 2 #2
w_ki = 0
w_kd = 0.01 #0
count =0
w_e_all=0
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7)
speed = Twist()
wich_x = 0
wich_y = 0
while not rospy.is_shutdown():
if target_x>0:
pid_x = w_pid_cal(target_x,x_f)
wich_x=x_f
if target_x<0:
pid_x = w_pid_cal(target_x,-x_b)
wich_x=-x_b
if target_y>0:
pid_y = w_pid_cal(target_y,y_l)
wich_y = y_l
if target_y<0:
pid_y = w_pid_cal(target_y,-y_r)
wich_y = -y_r
p_pid = p_pid_cal(target_yaw,yaw)
if abs(target_x-wich_x)<0.2 and abs(target_y-wich_y)<0.2:
speed.linear.x = 0
speed.linear.y = 0
else:
if abs(wich_x)>0.55:
speed.linear.y = 0.03*pid_y
else:
speed.linear.y = pid_y
speed.linear.x = pid_x
speed.angular.z = p_pid/180.0*3.14159265359
pid_vel_pub.publish(speed)
rate_loop_pid.sleep()
if vision_result != 0:
w_e_all=0
# rospy.sleep(0.3)
# thread_dis.join()
break
print("--------结束pid_go2--------")
def pid_turn(target_x,target_y,target_yaw):
global point_kp
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7.2)
speed = Twist()
while not rospy.is_shutdown():
map_pid_x = point_pid(target_x,now_pose_X)
map_pid_y = point_pid(target_y,now_pose_Y)
p_pid = p_pid_cal(target_yaw,yaw)
if target_yaw!=180:
speed.linear.x = 0
speed.linear.y = 0
else:
speed.linear.x = 0
speed.linear.y = 0
speed.angular.z = 1.5*p_pid/180.0*3.14159265359
pid_vel_pub.publish(speed)
rate_loop_pid.sleep()
# print(abs(target_yaw-yaw/3.1415926*180))
if abs(target_yaw-(yaw/3.1415926*180+180))<=50:
#rospy.sleep(0.3)
break
# -----------pid函数及其参数------------
# -----------获取视觉消息---------------
global object_vision
object_vision = 0
def vision_callback(msg):
global object_vision
if msg.data:
print(msg.data)
object_vision = msg.data
def vision_listen():
rospy.Subscriber('/detected_label',Int32,vision_callback,queue_size=10) #订阅视觉话题
rospy.spin()
global ar_sign
global task_vision
ar_sign = 0
def ar_callback(msg):
global object_vision , task_vision
ar_sign = 0
if len(msg.markers):
print("----------------------------------ar")
print(msg.markers[0].id)
task_vision = msg.markers[0].id
ar_sign = 1
def vision_ar():
rospy.Subscriber('/ar_pose_marker',AlvarMarkers,ar_callback,queue_size=10) #订阅视觉话题
rospy.spin()
global ai_vision
ai_vision = 0
global ai_sign
ai_sign = 0
def ai_callback(msg):
global object_vision , task_vision
if (msg.data):
print("++++++++++++++++++++++++++++++++++++ai")
print("ai回复",msg.data)
task_vision = msg.data
ai_sign = 1
def vision_ai():
rospy.Subscriber('/ai_result',Int32,ai_callback,queue_size=10) #订阅视觉话题
rospy.spin()
# -----------获取视觉消息---------------
#------------------- 雷达话题订阅线程 ----------------
global scan_data
scan_data = []
def get_valid_distance(scan, start_index, direction, angle_resolution):
"""
从指定的起始角度开始，以给定的方向和角度分辨率寻找有效的激光数据，
并计算修正后的距离。
参数：
- scan: 激光雷达扫描数据
- start_index: 起始角度索引
- direction: 搜索方向（1 表示顺时针，-1 表示逆时针）
- angle_resolution: 角度分辨率（每个索引代表的角度）
返回值：
- 修正后的有效距离
"""
max_angle = len(scan.ranges)
for i in range(max_angle):
index = (start_index + i * direction) % max_angle
if scan.ranges[index] != float('inf'):
distance = scan.ranges[index]
angle = np.radians((index - start_index) * angle_resolution)
distance_corrected = distance * np.cos(angle)
return distance_corrected
return float('inf')
def get_laserscan(scan):
"""
激光雷达回调函数，计算前后左右方向的有效激光距离并进行修正。
参数：
- scan: 激光雷达扫描数据
"""
global x_f, x_b, y_l, y_r, yaw, scan_data
scan_data = scan.ranges
front_index = 360 # 前方角度索引
angle_resolution = 0.5 # 每个索引代表的角度（假设为0.5度）
# 找到有效距离并进行修正
x_f = get_valid_distance(scan, front_index, 1, angle_resolution) # 从前方开始向右搜索
x_b = get_valid_distance(scan, 0, 1, angle_resolution) # 从后方开始向右搜索
y_l = get_valid_distance(scan, 540, -1, angle_resolution) # 从左侧开始向左搜索
y_r = get_valid_distance(scan, 180, 1, angle_resolution) # 从右侧开始向右搜索
#print("x_f:", x_f, "x_b:", x_b, "y_l:", y_l, "y_r:", y_r)
def laser_listen():
rospy.Subscriber('/scan_filtered', LaserScan,get_laserscan,queue_size=7)
rospy.spin()
#------------------- 雷达话题订阅线程 ----------------
# -----------------------pid回家--------------------------
def pid_loop():
print("---------启动回家--------")
global yaw
global w_kp,w_ki,w_kd,w_e_all
w_e_all=0
w_kp = 0.8 #2
w_ki = 0
w_kd = 0.00 #0
pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7)
rate_loop_pid=rospy.Rate(7)
speed = Twist()
while not rospy.is_shutdown():
pid_x = w_pid_cal(0.155,x_f)
#pid_y = w_pid_cal(-0.35,-y_r)
pid_y = w_pid_cal(0.155,y_l)
p_pid = p_pid_cal(0,yaw)
print("x_f",x_f)
print("y_l",y_l)
print("yaw",yaw)
speed.linear.x = pid_x
speed.linear.y = pid_y
speed.angular.z = p_pid/180.0*3.14159265359
pid_vel_pub.publish(speed)
rate_loop_pid.sleep()
if abs(x_f-0.1)<0.18 and abs(y_l-0.1)<0.18 and abs(0-(yaw/3.1415926*180+180))<=5:
speed.linear.x = 0
speed.linear.y = 0
pid_vel_pub.publish(speed)
break
print("---------结束回家--------")
# -----------------------pid回家--------------------------
# ---------------vioce 语音播报-----------------
def play_voice(number):
playsound(str(number)+".mp3")
def play_voice_begin(numbers):
print("获取的任务目标为:"+str(target_point_arr))
numbers=sorted(numbers)
playsound(str(numbers[0])+"_"+str(numbers[1])+"_"+str(numbers[2])+".mp3")
print("播放文件为："+str(numbers[0])+"_"+str(numbers[1])+"_"+str(numbers[2])+".mp3")
# ---------------vioce 语音播报-----------------
# -----------------导航点----------------------
# 1.本区域共三个函数导航实时距离获取，发布导航点，控制导航点发布
# 2.控制导航点发布部分默认是逐一发布点列表中的点，需要请注释并自行更改
# 发布目标点
# 控制导航点发布
global count_dis_times
count_dis_times=0
def dis_func(x, y,min_dis):
global dis_trun_off,now_pose_X,now_pose_Y,distance, vision_result,target_point_arr,times_voice_play,count_dis_times
print("dis_func函数已启动:"+str(count_dis_times)+"次")
count_dis_times+=1
# lock = threading.RLock()
dis_fun_rate=rospy.Rate(10)
while not rospy.is_shutdown():
dis_fun_rate.sleep()
car_to_map_x=now_pose_X
car_to_map_y=now_pose_Y
# 计算小车到目标点的距离
distance = pow(pow( car_to_map_x - x, 2) + pow(car_to_map_y - y, 2), 0.5)
#print("distance:"+str(distance))
if distance<min_dis:
print("reach_goal")
rospy.sleep(1)
break
def goals(x, y, i):
min_dis=0.07
global qtn_list_xy,move_base,dis_trun_off,now_pose_X,pid_stop_vision_stop_flag,goal_vision_weight_flag
target_point = Pose(Point(x, y, 0), Quaternion(qtn_list_xy[i][0], qtn_list_xy[i][1], qtn_list_xy[i][2], qtn_list_xy[i][3]))
goal = MoveBaseGoal()
goal.target_pose.pose = target_point
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Going to: " + str(target_point))
print("goal")
move_base.send_goal(goal)
def goals_task(x, y, i):
min_dis=0.07
global qtn_list_task_xy,move_base,dis_trun_off,now_pose_X
target_point = Pose(Point(x, y, 0), Quaternion(qtn_list_task_xy[i][0], qtn_list_task_xy[i][1], qtn_list_task_xy[i][2], qtn_list_task_xy[i][3]))
goal = MoveBaseGoal()
goal.target_pose.pose = target_point
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Going to: " + str(target_point))
print("goal")
move_base.send_goal(goal)
def send_None_goal():
global move_base
goal = MoveBaseGoal()
move_base.send_goal(goal)
# ---------------vioce 语音播报-----------------
global times_voice_play
times_voice_play=0
def play_voice(number):
global times_voice_play
playsound(str(number)+".mp3")
# ---------------vioce 语音播报-----------------
# ----------------- init ---------------------
# 1.处理点列表的四元数并放在新的列表
# 2.连接move_base
# --------------
def now_pose_xy():
global now_pose_X,now_pose_Y,yaw
now_pose=rospy.Rate(10)
listener = tf.TransformListener()
while not rospy.is_shutdown():
now_pose.sleep()
try:
(trans, rot) = listener.lookupTransform("map", "base_link", rospy.Time(0))
# 小车坐标
now_pose_X=trans[0]
now_pose_Y=trans[1]
euler = tf.transformations.euler_from_quaternion(rot)
yaw = euler[2] # 第三个元素是yaw角
# print("x",now_pose_X,"y",now_pose_Y,"yaw",yaw)
except Exception as e:
print(".........")
# --------------
def init_fun():
#转换点
global qtn_list_xy,qtn_list_task_xy,pose_num_xy,pose_num_task_xy,move_base
for i in range(pose_num_xy):
qtn = tf.transformations.quaternion_from_euler(0,0,xy[i][2])
qtn_list_xy.append(qtn)
i=0
for i in range(pose_num_task_xy):
qtn = tf.transformations.quaternion_from_euler(0,0,task_xy[i][2])
qtn_list_task_xy.append(qtn)
#连接move_base—actition
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
rospy.loginfo("Request to connect to move_base server")
rospy.loginfo("Be connected successfully")
thread_lidar = threading.Thread(target=laser_listen)
thread_lidar.start()
thread_vision = threading.Thread(target=vision_listen)
thread_vision.start()
thread_ar = threading.Thread(target=vision_ar)
thread_ar.start()
thread_ai = threading.Thread(target=vision_ai)
thread_ai.start()
thread_now_pose = threading.Thread(target=now_pose_xy)
thread_now_pose.start()
# 初始化 history 变量
history = {"current_value": None, "stable_count": 0, "values": []}
required_stable_count = 20
def get_stable_value(new_value, required_stable_count, history, max_history_size=50):
# 更新历史记录列表，保留最近的 max_history_size 个值
history["values"].append(new_value)
if len(history["values"]) > max_history_size:
history["values"].pop(0) # 超过最大限制时移除最早的值
# 检查稳定计数
if new_value == history["current_value"]:
history["stable_count"] += 1
else:
history["current_value"] = new_value
history["stable_count"] = 1
# 如果达到所需的稳定次数，返回稳定的值
if history["stable_count"] >= required_stable_count:
return history["current_value"]
else:
return None
def vision_to_get_task(t_long):
global object_vision , ar_sign , task_vision,ai_sign
task_vision = 0
object_vision = 0
ar_sign=0
ai_sign = 0
t = 0
sure_required = 0
rate_loop_pid=rospy.Rate(10)
rospy.set_param('/top_view_shot_node/im_flag', 1)
rospy.sleep(3)
while not rospy.is_shutdown():
print("find.............",object_vision)
if object_vision and not task_vision:
sure_required = get_stable_value(object_vision, 50, history)
if sure_required is not None:
print("sure_required",sure_required)
task_vision = sure_required
t+=1
if t>=t_long:
print("超时")
task_vision=999
break
if task_vision:
print("========",task_vision)
oject_vision = 0
break
rate_loop_pid.sleep()
return task_vision
def get_voice(voice):
global target_2
global target_3
if (target_2==0 or target_3==0):
target_2 = voice.er
target_3 = voice.san
rospy.loginfo("收到/target话题的反馈: %d %d",target_2, target_3) # 访问自定义消息的字段
def robot_voice():
rospy.Subscriber('/target',Targettt,get_voice,queue_size=1)
rospy.spin()
def voice_wakeup_publisher():
pub = rospy.Publisher('/voiceWakeup', String, queue_size=10)
user_input = input("请输入1开始启动: ")
if user_input == "1":
msg = String()
msg.data = "1"
pub.publish(msg)
rospy.loginfo("已发布消息: %s", msg.data)
# ----------------- init ---------------------
if __name__ == '__main__':
# 初始化节点
rospy.init_node('move_test', anonymous=True)
init_fun()
a = input()
rospy.loginfo("节点初始化完成，开始执行任务")
# 发布唤醒信号
#voice_wakeup_publisher()
goals(xy[0][0], xy[0][1], 0)
dis_func(xy[0][0], xy[0][1], 0.05)
send_None_goal()
pid_stop2(pid_stop_xy[0][0], pid_stop_xy[0][1], pid_stop_xy[0][2])
rospy.sleep(5)
'''goals(xy[1][0], xy[1][1], 1)
dis_func(xy[0][0], xy[0][1], 0.05)
send_None_goal()
pid_stop2(pid_stop_xy[1][0], pid_stop_xy[1][1], pid_stop_xy[1][2])
#rospy.loginfo("任务2完成")
rospy.sleep(5)'''
'''goals_task(task_xy[0][0], task_xy[0][1], 0)
dis_func(task_xy[0][0], task_xy[0][1], 0.05) # 等待到达目标点附近
send_None_goal()
pid_stop2(pid_stop_xy[2][0], pid_stop_xy[2][1], pid_stop_xy[2][2])
rospy.sleep(4)'''
rospy.loginfo("任务序列执行完成")
我完整代码是这个应该怎么修改
本文详细解析了Java中synchronized关键字与wait方法的使用区别，特别是针对不同对象锁的释放情况进行了深入探讨。
扫一扫
688
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
