ROS 毕设 仓库环境 小车自主移动 2

给一个“小朋友”做的毕设-2
在1中，我们已经建立了仓库环境，现在开始巡逻仓库。

仓库巡逻

目标移动位置

先提前设置几个目标移动点如下（存储为txt文件）：

A [4,-4,0,0,0,0,1]
B [-7,1.7,0,0,0,0,1]
C [-6,-2,0,0,0,0,1]

以上位姿数据分别表示：position(x,y,z) orientation(x,y,z,w)

在你的项目中可以更具自身需求让这一部分的数据变得有实际意义
（比如我设置的几个点在环境中分别是货架、垃圾桶、工作台）

轮询代码

publish_pose_stamped.py

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped

# 读数据
filename = 'target-position.txt' # 这里最好用绝对地址

with open(filename, 'r') as file:
    data = file.read()

pos = data.split('\\n')
pos_dict = {}
for i in range(len(pos)):
    pos_dict[pos[i][0]] = pos[i][3:-1]

def publish_pose_stamped():
    rospy.init_node('publish_pose_stamped_node', anonymous=True)
    pub = rospy.Publisher('/move_base_simple/goal ', PoseStamped, queue_size=10)
    rate = rospy.Rate(1)  # 1 Hz

    while not rospy.is_shutdown():
        # Create a PoseStamped message
        pose_stamped_msg = PoseStamped()
        pose_stamped_msg.header.stamp = rospy.Time.now()
        pose_stamped_msg.header.frame_id = 'map'

        target = input('请输入要移动的目标位置（A/B/C）：')

        if target.upper() == 'A':
        # 发送移动到A点的指令
            print('A')
        elif target.upper() == 'B':
            # 发送移动到B点的指令
            print('B')
        elif target.upper() == 'C':
            # 发送移动到C点的指令
            print('C')
        elif target.upper() == '0':
            # 推出
            print('已退出')
            break
        else:
            print('invalid')

        # Create a PoseStamped message
        pose_stamped_msg = PoseStamped()
        pose_stamped_msg.header.stamp = rospy.Time.now()
        pose_stamped_msg.header.frame_id = map
        target = pos_dict[target].split(',')
        pose_stamped_msg.pose.position.x = float(target[0])
        pose_stamped_msg.pose.position.y = float(target[1])
        pose_stamped_msg.pose.position.z = float(target[2])
        pose_stamped_msg.pose.orientation.x = float(target[3])
        pose_stamped_msg.pose.orientation.y = float(target[4])
        pose_stamped_msg.pose.orientation.z = float(target[5])
        pose_stamped_msg.pose.orientation.w = float(target[6])

        # Publish the message
        pub.publish(pose_stamped_msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_pose_stamped()
    except rospy.ROSInterruptException:
        pass

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(publish_pose_stamped)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  geometry_msgs
)

catkin_package()

package.xml

<?xml version=1.0?>
<package format=2>
  <name>publish_pose_stamped</name>
  <version>0.0.0</version>
  <description>The publish_pose_stamped package</description>

  <license>TODO</license>
  <maintainer email=you@example.com>Your Name</maintainer>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>geometry_msgs</run_depend>

  <export>
  </export>
</package>

编译

运行以下命令启动ROS节点：

roslaunch turtlebot3_gazebo turtlebot3_world.launch

roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=/home/xiaoge4/catkin_turtlebot3/src/map/map.yaml

rosrun publish_pose_stamped publish_pose_stamped.py

根据你的移动目的输入 A（或B C等）回车，小车自主移动到指定位置。

实现结果

B站视频

ROS 仓库环境 小车自主移动