catkin_ws/
└── src/
└── my_robot_pkg/
├── launch/ # 启动文件目录
│ ├── simulation.launch
├── scripts/ # Python脚本
│ ├── move_to_
target.py
│ └── input_node.py
├── urdf/ # URDF/XACRO文件
│ ├── four_wheel_robot.xacro
├── worlds/ # Gazebo世界文件
│ └── four_areas.world
├── CMakeLists.txt # 编译规则
└── package.xml # 包配置
simulation.launch
<launch>
<!-- Gazebo 仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find my_robot_pkg)/worlds/four_areas.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- 加载机器人模型 -->
<param name="robot_description"
command="$(find xacro)/xacro $(find my_robot_pkg)/urdf/four_wheel_robot.xacro" />
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model my_robot
-x 0 -y 0 -z 0.1 -R 0 -P 0 -Y 0" />
<!-- 控制节点 -->
<node name="navigation_controller" pkg="my_robot_pkg"
type="move_to_
target.py" output="screen"/>
<!-- 输入节点 -->
<node name="
target_input" pkg="my_robot_pkg"
type="input_node.py" output="screen"/>
<!-- RViz 配置 -->
<node name="rviz" pkg="rviz" type="rviz"
args="-d $(find my_robot_pkg)/rviz/nav_config.rviz" />
<!-- TF 转换 -->
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
</launch>
input_node.py
#!/usr/bin/env python3
import rospy
from std_msgs.msg import Int32
class
TargetInput:
def __init__(
self):
rospy.init_node('
target_input', anonymous=True)
self.pub = rospy.Publisher('/
target_area', Int32, queue_size=10)
# 显示指令提示
print("""
******************************
* 输入目标区域编号 (1-4):
* 1: 红色区域 (1, 0)
* 2: 绿色区域 (2, 0)
* 3: 蓝色区域 (3, 0)
* 4: 黄色区域 (4, 0)
* 输入q退出
******************************
""")
def run(
self):
while not rospy.is_shutdown():
try:
user_input = input("请输入目标编号 (1-4): ")
if user_input.lower() == 'q':
rospy.signal_shutdown("User exit")
break
target = int(user_input)
if 1 <=
target <= 4:
self.pub.publish(
target)
rospy.loginfo(f"已发送目标 {
target}")
else:
print("错误：请输入1-4之间的数字")
except ValueError:
print("错误：无效输入，请输入数字")
except rospy.ROSInterruptException:
break
if __name__ == '__main__':
try:
node =
TargetInput()
node.run()
except rospy.ROSInterruptException:
pass
move_to_
target.py
#!/usr/bin/env python3
import rospy
import math
import tf
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32
class NavigationController:
def __init__(
self):
rospy.init_node('navigation_controller', anonymous=True)
# PID参数
self.KP_linear = 0.5 # 线速度比例系数
self.KP_angular = 1.2 # 角速度比例系数
self.goal_tolerance = 0.1 # 到达目标的允许误差
# 初始化变量
self.
target_position = None
self.current_pose = None
# 订阅和发布
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.Subscriber('/
target_area', Int32,
self.
target_callback)
rospy.Subscriber('/odom', Odometry,
self.odom_callback)
rospy.loginfo("Navigation controller initialized")
def
target_callback(
self, msg):
"""处理目标区域输入"""
target_dict = {
1: (1.0, 0.0),
2: (2.0, 0.0),
3: (3.0, 0.0),
4: (4.0, 0.0)
}
self.
target_position =
target_dict.get(msg.data)
if
self.
target_position:
rospy.loginfo(f"New
target received: {
self.
target_position}")
def odom_callback(
self, msg):
"""更新当前位姿"""
self.current_pose = msg.pose.pose
def get_yaw(
self):
"""从四元数获取偏航角"""
if
self.current_pose:
orientation =
self.current_pose.orientation
quaternion = (
orientation.x,
orientation.y,
orientation.z,
orientation.w
)
euler = tf.transformations.euler_from_quaternion(quaternion)
return euler[2] # 返回偏航角（绕Z轴旋转）
return 0.0
def run(
self):
rate = rospy.Rate(10) # 10Hz
while not rospy.is_shutdown():
if
self.
target_position and
self.current_pose:
# 获取当前位置
current_x =
self.current_pose.position.x
current_y =
self.current_pose.position.y
# 计算距离差
dx =
self.
target_position[0] - current_x
dy =
self.
target_position[1] - current_y
distance = math.hypot(dx, dy)
if distance >
self.goal_tolerance:
# 计算目标角度
target_yaw = math.atan2(dy, dx)
current_yaw =
self.get_yaw()
# 计算角度差（归一化到[-π, π]）
yaw_error =
target_yaw - current_yaw
if yaw_error > math.pi:
yaw_error -= 2 * math.pi
elif yaw_error < -math.pi:
yaw_error += 2 * math.pi
# 生成控制指令
twist = Twist()
if abs(yaw_error) > 0.1: # 优先调整方向
twist.angular.z =
self.KP_angular * yaw_error
twist.linear.x = 0.0
else: # 方向正确后前进
twist.linear.x = min(
self.KP_linear * distance, 0.3) # 限制最大速度
twist.angular.z = 0.0
self.cmd_vel_pub.publish(twist)
else:
# 到达目标，停止运动
self.cmd_vel_pub.publish(Twist())
self.
target_position = None
rospy.loginfo("
Target reached!")
rate.sleep()
if __name__ == '__main__':
try:
controller = NavigationController()
controller.run()
except rospy.ROSInterruptException:
pass
four_wheel_robot.xacro
<?xml version="1.0"?>
<robot name="four_wheel_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- ################### 参数化定义 ################### -->
<!-- 基础参数 -->
<xacro:property name="base_length" value="0.4" /> <!-- X轴方向长度 -->
<xacro:property name="base_width" value="0.3" /> <!-- Y轴方向宽度 -->
<xacro:property name="base_height" value="0.2" /> <!-- Z轴方向高度 -->
<!-- 轮子参数 -->
<xacro:property name="wheel_radius" value="0.05" /> <!-- 轮子半径 -->
<xacro:property name="wheel_thickness" value="0.04" /> <!-- 轮子厚度 -->
<!-- 驱动参数 -->
<xacro:property name="wheel_separation" value="${base_width}" /> <!-- 轮间距 -->
<xacro:property name="wheel_diameter" value="${2*wheel_radius}" /> <!-- 轮直径 -->
<!-- ################### 虚拟根链接 ################### -->
<link name="dummy_base">
<inertial>
<mass value="0.001" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<joint name="dummy_joint" type="fixed">
<parent link="dummy_base" />
<child link="base_link" />
</joint>
<!-- ################### 主体结构 ################### -->
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}" />
</geometry>
<material name="base_color">
<color rgba="0 0 0.8 1" />
</material>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}" />
</geometry>
<surface>
<friction>
<ode>
<mu>0.5</mu>
<mu2>0.5</mu2>
</ode>
</friction>
</surface>
</collision>
<inertial>
<mass value="5.0" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
iyy="0.1" iyz="0.0" izz="0.1" />
</inertial>
</link>
<!-- ################### 轮子宏定义 ################### -->
<xacro:macro name="wheel" params="prefix pos_x pos_y">
<link name="${prefix}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_thickness}" />
</geometry>
<material name="wheel_black" />
</visual>
<collision>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_thickness}" />
</geometry>
<surface>
<friction>
<ode>
<mu>1.0</mu>
<mu2>1.0</mu2>
</ode>
</friction>
</surface>
</collision>
<inertial>
<mass value="0.5" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link" />
<child link="${prefix}_wheel" />
<origin xyz="${pos_x} ${pos_y} -${base_height/2}"
rpy="${math.pi/2} 0 0" />
<axis xyz="0 0 1" />
</joint>
</xacro:macro>
<!-- ################### 轮子安装 ################### -->
<xacro:wheel prefix="front_left"
pos_x="${base_length/2 - wheel_radius}"
pos_y="${base_width/2}" />
<xacro:wheel prefix="front_right"
pos_x="${base_length/2 - wheel_radius}"
pos_y="-${base_width/2}" />
<xacro:wheel prefix="rear_left"
pos_x="-${base_length/2 + wheel_radius}"
pos_y="${base_width/2}" />
<xacro:wheel prefix="rear_right"
pos_x="-${base_length/2 + wheel_radius}"
pos_y="-${base_width/2}" />
<!-- ################### Gazebo扩展 ################### -->
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
</sensor>
</gazebo>
<gazebo>
<plugin name="differential_drive" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishOdomTF>true</publishOdomTF>
<wheelSeparation>${wheel_separation}</wheelSeparation>
<wheelDiameter>${wheel_diameter}</wheelDiameter>
<maxWheelTorque>20</maxWheelTorque>
<publishWheelTF>true</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>
</robot>
four_areas.world
<?xml version="1.0"?>
<sdf version="1.9"> <!-- 升级SDF版本 -->
<world name="four_areas">
<!-- 地面 -->
<include>
<uri>model://ground_plane</uri>
</include>
<!-- 光照 -->
<include>
<uri>model://sun</uri>
<name>main_sun</name> <!-- 添加唯一名称 -->
</include>
<!-- 区域1 (红色) -->
<model name="area_1">
<static>true</static>
<link name="area1_link"> <!-- 修改链接名称 -->
<visual name="area1_visual"> <!-- 添加唯一名称 -->
<geometry>
<box><size>0.5 0.5 0.01</size></box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name> <!-- 使用Gazebo内置材质 -->
</script>
</material>
</visual>
</link>
<pose>1.0 0.0 0.005 0 0 0</pose>
</model>
<!-- 区域2 (绿色) -->
<model name="area_2">
<static>true</static>
<link name="area2_link">
<visual name="area2_visual">
<geometry>
<box><size>0.5 0.5 0.01</size></box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
</visual>
</link>
<pose>2.0 0.0 0.005 0 0 0</pose>
</model>
<!-- 区域3 (蓝色) -->
<model name="area_3">
<static>true</static>
<link name="area3_link">
<visual name="area3_visual">
<geometry>
<box><size>0.5 0.5 0.01</size></box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Blue</name>
</script>
</material>
</visual>
</link>
<pose>3.0 0.0 0.005 0 0 0</pose>
</model>
<!-- 区域4 (黄色) -->
<model name="area_4">
<static>true</static>
<link name="area4_link">
<visual name="area4_visual">
<geometry>
<box><size>0.5 0.5 0.01</size></box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Yellow</name>
</script>
</material>
</visual>
</link>
<pose>4.0 0.0 0.005 0 0 0</pose>
</model>
</world>
</sdf>
利用ros设置仿真四轮小车，并且有仿真环境，仿真环境中有1、2、3、4四个小区域，能输入1、2、3、4让小车跑到相对应的1、2、3、4位置区域，RVIZ可视化来看结果,写出相关代码，和相关文件包含示例.查看上述代码并完善
本文讨论了在进行文件下载操作时移除<base>标签的必要性，特别是在Internet Explorer中避免弹窗问题的方法。对于现代浏览器如火狐和谷歌浏览器，由于它们不支持showModalDialog和showModelessDialog，因此不受此问题影响。
扫一扫
4162
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
