import sys
import timefrom fontTools.merge import timer
from ipykernel.heartbeat import Heartbeatdef OdomCallback(msg):print("腿部里程计调用")global body_x, body_yclass Move_control():def __init__(self):# 连接机器狗的运动主机及其端口self.server_address = ("192.168.1.120", 43893)self.engine = pyttsx3.init()self.speak_engine()# 速度发布 话题是cmd_vel；消息类型是Twistself.vel_cmd = Twist()self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)# 速度设置函数def vel_cmd_config(self, x=0.0, y=0.0, z=0.0):self.vel_cmd.linear.x = xself.vel_cmd.linear.y = y# angular是控制狗的旋转self.vel_cmd.angular.z = zself.vel_pub.publish(self.vel_cmd)if __name__ == '__main__':# 初始化ROS节点rospy.init_node('move_node')# 订阅腿部里程计leg_odom_sub = rospy.Subscriber('leg_odom', PoseWithCovarianceStamped, queue_size=10)# 发送信号包，让狗能够持续受到控制heart = Heartbeat()time.sleep(1)print("Wait 5 seconds and stand up......")# 狗起立了robot_controller.stand_up()# 如果没有起立，那么就提示应该重启程序print("Now,dog should stand up, otherwise press 'ctrl + c' to shut down and re-run the demo")# 发送自主模式指令robot_controller.auto_control()# 输入指令开始运行run_flag = int(input("输入1开始运行:"))# 如果输入的不是1，那么直接关闭程序if run_flag != 1:sys.exit()# 初始点target = -1# 初始化计时time_threshold = 20timer = time.time()try:while not rospy.is_shutdowm():if target == 0:if body_x < 0.85:# 当x方向没有到0.85的时候，发送x方向的速度：0.15move_controller.vel_cmd_config(0.15, 0)if body_x >= 0.85:time.sleep(3)move_controller.vel_cmd_config(0, 0)print("开启越障模式了")# 开启越障模式move_controller.vel_cmd_config(0.15, 0)if body_x >= 1.3:move_controller.vel_cmd_config(0, 0)# 关闭避障模式# 继续前进到转向点move_controller.vel_cmd_config(0.15, 0)target = 1elif target == 1:python实现机器狗的行动控制
