feat: add all code
This commit is contained in:
鱼香ROS
28
chapt7/chapt7_ws/src/autopatrol_interfaces/CMakeLists.txt
Normal file
28
chapt7/chapt7_ws/src/autopatrol_interfaces/CMakeLists.txt
Normal file
@@ -0,0 +1,28 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(autopatrol_interfaces)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/SpeachText.srv"
|
||||
)
|
||||
ament_package()
|
||||
18
chapt7/chapt7_ws/src/autopatrol_interfaces/package.xml
Normal file
18
chapt7/chapt7_ws/src/autopatrol_interfaces/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>autopatrol_interfaces</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,3 @@
|
||||
string text # 合成文字
|
||||
---
|
||||
bool result # 合成结果
|
||||
@@ -0,0 +1,173 @@
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped, Pose
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
from tf2_ros import TransformListener, Buffer
|
||||
from tf_transformations import euler_from_quaternion, quaternion_from_euler
|
||||
from rclpy.duration import Duration
|
||||
# 添加服务接口
|
||||
from autopatrol_interfaces.srv import SpeachText
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
|
||||
class PatrolNode(BasicNavigator):
|
||||
def __init__(self, node_name='patrol_node'):
|
||||
super().__init__(node_name)
|
||||
# 导航相关定义
|
||||
self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57])
|
||||
self.initial_point_ = self.get_parameter('initial_point').value
|
||||
self.target_points_ = self.get_parameter('target_points').value
|
||||
# 实时位置获取 TF 相关定义
|
||||
self.buffer_ = Buffer()
|
||||
self.listener_ = TransformListener(self.buffer_, self)
|
||||
self.speach_client_ = self.create_client(SpeachText, 'speech_text')
|
||||
|
||||
# 订阅与保存图像相关定义
|
||||
self.declare_parameter('image_save_path', '')
|
||||
self.image_save_path = self.get_parameter('image_save_path').value
|
||||
self.bridge = CvBridge()
|
||||
self.latest_image = None
|
||||
self.subscription_image = self.create_subscription(
|
||||
Image, '/camera_sensor/image_raw', self.image_callback, 10)
|
||||
|
||||
def image_callback(self, msg):
|
||||
"""
|
||||
将最新的消息放到 latest_image 中
|
||||
"""
|
||||
self.latest_image = msg
|
||||
|
||||
def record_image(self):
|
||||
"""
|
||||
记录图像
|
||||
"""
|
||||
if self.latest_image is not None:
|
||||
pose = self.get_current_pose()
|
||||
cv_image = self.bridge.imgmsg_to_cv2(self.latest_image)
|
||||
cv2.imwrite(f'{self.image_save_path}image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image)
|
||||
|
||||
|
||||
def speach_text(self, text):
|
||||
"""
|
||||
调用服务播放语音
|
||||
"""
|
||||
while not self.speach_client_.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().info('语合成服务未上线，等待中。。。')
|
||||
|
||||
request = SpeachText.Request()
|
||||
request.text = text
|
||||
future = self.speach_client_.call_async(request)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
if future.result() is not None:
|
||||
result = future.result().result
|
||||
if result:
|
||||
self.get_logger().info(f'语音合成成功：{text}')
|
||||
else:
|
||||
self.get_logger().warn(f'语音合成失败：{text}')
|
||||
else:
|
||||
self.get_logger().warn('语音合成服务请求失败')
|
||||
|
||||
def get_pose_by_xyyaw(self, x, y, yaw):
|
||||
"""
|
||||
通过 x,y,yaw 合成 PoseStamped
|
||||
"""
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = 'map'
|
||||
pose.pose.position.x = x
|
||||
pose.pose.position.y = y
|
||||
rotation_quat = quaternion_from_euler(0, 0, yaw)
|
||||
pose.pose.orientation.x = rotation_quat[0]
|
||||
pose.pose.orientation.y = rotation_quat[1]
|
||||
pose.pose.orientation.z = rotation_quat[2]
|
||||
pose.pose.orientation.w = rotation_quat[3]
|
||||
return pose
|
||||
|
||||
def init_robot_pose(self):
|
||||
"""
|
||||
初始化机器人位姿
|
||||
"""
|
||||
# 从参数获取初始化点
|
||||
self.initial_point_ = self.get_parameter('initial_point').value
|
||||
# 合成位姿并进行初始化
|
||||
self.setInitialPose(self.get_pose_by_xyyaw(
|
||||
self.initial_point_[0], self.initial_point_[1], self.initial_point_[2]))
|
||||
# 等待直到导航激活
|
||||
self.waitUntilNav2Active()
|
||||
|
||||
def get_target_points(self):
|
||||
"""
|
||||
通过参数值获取目标点集合
|
||||
"""
|
||||
points = []
|
||||
self.target_points_ = self.get_parameter('target_points').value
|
||||
for index in range(int(len(self.target_points_)/3)):
|
||||
x = self.target_points_[index*3]
|
||||
y = self.target_points_[index*3+1]
|
||||
yaw = self.target_points_[index*3+2]
|
||||
points.append([x, y, yaw])
|
||||
self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})')
|
||||
return points
|
||||
|
||||
def nav_to_pose(self, target_pose):
|
||||
"""
|
||||
导航到指定位姿
|
||||
"""
|
||||
self.waitUntilNav2Active()
|
||||
result = self.goToPose(target_pose)
|
||||
while not self.isTaskComplete():
|
||||
feedback = self.getFeedback()
|
||||
if feedback:
|
||||
self.get_logger().info(f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达')
|
||||
# 最终结果判断
|
||||
result = self.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
self.get_logger().info('导航结果：成功')
|
||||
elif result == TaskResult.CANCELED:
|
||||
self.get_logger().warn('导航结果：被取消')
|
||||
elif result == TaskResult.FAILED:
|
||||
self.get_logger().error('导航结果：失败')
|
||||
else:
|
||||
self.get_logger().error('导航结果：返回状态无效')
|
||||
|
||||
def get_current_pose(self):
|
||||
"""
|
||||
通过TF获取当前位姿
|
||||
"""
|
||||
while rclpy.ok():
|
||||
try:
|
||||
tf = self.buffer_.lookup_transform(
|
||||
'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
|
||||
transform = tf.transform
|
||||
rotation_euler = euler_from_quaternion([
|
||||
transform.rotation.x,
|
||||
transform.rotation.y,
|
||||
transform.rotation.z,
|
||||
transform.rotation.w
|
||||
])
|
||||
self.get_logger().info(
|
||||
f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
|
||||
return transform
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
patrol = PatrolNode()
|
||||
patrol.speach_text(text='正在初始化位置')
|
||||
patrol.init_robot_pose()
|
||||
patrol.speach_text(text='位置初始化完成')
|
||||
|
||||
while rclpy.ok():
|
||||
for point in patrol.get_target_points():
|
||||
x, y, yaw = point[0], point[1], point[2]
|
||||
# 导航到目标点
|
||||
target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)
|
||||
patrol.speach_text(text=f'准备前往目标点{x},{y}')
|
||||
patrol.nav_to_pose(target_pose)
|
||||
patrol.speach_text(text=f"已到达目标点{x},{y},准备记录图像")
|
||||
patrol.record_image()
|
||||
patrol.speach_text(text=f"图像记录完成")
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,26 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from autopatrol_interfaces.srv import SpeachText
|
||||
import espeakng
|
||||
|
||||
class Speaker(Node):
|
||||
def __init__(self, node_name):
|
||||
super().__init__(node_name)
|
||||
self.speech_service = self.create_service(
|
||||
SpeachText, 'speech_text', self.speak_text_callback)
|
||||
self.speaker = espeakng.Speaker()
|
||||
self.speaker.voice = 'zh'
|
||||
|
||||
def speak_text_callback(self, request, response):
|
||||
self.get_logger().info('正在朗读 %s' % request.text)
|
||||
self.speaker.say(request.text)
|
||||
self.speaker.wait()
|
||||
response.result = True
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = Speaker('speaker')
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
@@ -0,0 +1,10 @@
|
||||
patrol_node:
|
||||
ros__parameters:
|
||||
initial_point: [0.0, 0.0, 0.0]
|
||||
target_points: [
|
||||
0.0,0.0,0.0,
|
||||
1.0,2.0,3.14,
|
||||
-4.5,1.5,1.57,
|
||||
-8.0,-5.0,1.57,
|
||||
1.0,-5.0,3.14,
|
||||
]
|
||||
@@ -0,0 +1,28 @@
|
||||
import os
|
||||
import launch
|
||||
import launch_ros
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取与拼接默认路径
|
||||
autopatrol_robot_dir = get_package_share_directory(
|
||||
'autopatrol_robot')
|
||||
patrol_config_path = os.path.join(
|
||||
autopatrol_robot_dir, 'config', 'patrol_config.yaml')
|
||||
|
||||
action_node_turtle_control = launch_ros.actions.Node(
|
||||
package='autopatrol_robot',
|
||||
executable='patrol_node',
|
||||
parameters=[patrol_config_path]
|
||||
)
|
||||
action_node_patrol_client = launch_ros.actions.Node(
|
||||
package='autopatrol_robot',
|
||||
executable='speaker',
|
||||
)
|
||||
|
||||
return launch.LaunchDescription([
|
||||
action_node_turtle_control,
|
||||
action_node_patrol_client,
|
||||
])
|
||||
18
chapt7/chapt7_ws/src/autopatrol_robot/package.xml
Normal file
18
chapt7/chapt7_ws/src/autopatrol_robot/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>autopatrol_robot</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
4
chapt7/chapt7_ws/src/autopatrol_robot/setup.cfg
Normal file
4
chapt7/chapt7_ws/src/autopatrol_robot/setup.cfg
Normal file
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/autopatrol_robot
|
||||
[install]
|
||||
install_scripts=$base/lib/autopatrol_robot
|
||||
29
chapt7/chapt7_ws/src/autopatrol_robot/setup.py
Normal file
29
chapt7/chapt7_ws/src/autopatrol_robot/setup.py
Normal file
@@ -0,0 +1,29 @@
|
||||
from setuptools import find_packages, setup
|
||||
from glob import glob
|
||||
package_name = 'autopatrol_robot'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name+'/config', ['config/patrol_config.yaml']),
|
||||
('share/' + package_name+'/launch', glob('launch/*.launch.py')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='fishros',
|
||||
maintainer_email='87068644+fishros@users.noreply.github.com',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'patrol_node=autopatrol_robot.patrol_node:main',
|
||||
'speaker=autopatrol_robot.speaker:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
25
chapt7/chapt7_ws/src/autopatrol_robot/test/test_copyright.py
Normal file
25
chapt7/chapt7_ws/src/autopatrol_robot/test/test_copyright.py
Normal file
@@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
25
chapt7/chapt7_ws/src/autopatrol_robot/test/test_flake8.py
Normal file
25
chapt7/chapt7_ws/src/autopatrol_robot/test/test_flake8.py
Normal file
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
23
chapt7/chapt7_ws/src/autopatrol_robot/test/test_pep257.py
Normal file
23
chapt7/chapt7_ws/src/autopatrol_robot/test/test_pep257.py
Normal file
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
@@ -0,0 +1,40 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros import TransformListener, Buffer
|
||||
from tf_transformations import euler_from_quaternion
|
||||
|
||||
|
||||
class TFListener(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('tf2_listener')
|
||||
self.buffer = Buffer()
|
||||
self.listener = TransformListener(self.buffer, self)
|
||||
self.timer = self.create_timer(1, self.get_transform)
|
||||
|
||||
def get_transform(self):
|
||||
try:
|
||||
tf = self.buffer.lookup_transform(
|
||||
'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
|
||||
transform = tf.transform
|
||||
rotation_euler = euler_from_quaternion([
|
||||
transform.rotation.x,
|
||||
transform.rotation.y,
|
||||
transform.rotation.z,
|
||||
transform.rotation.w
|
||||
])
|
||||
self.get_logger().info(
|
||||
f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = TFListener()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,21 @@
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator
|
||||
import rclpy
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
navigator = BasicNavigator()
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 0.0
|
||||
initial_pose.pose.position.y = 0.0
|
||||
initial_pose.pose.orientation.w = 1.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
navigator.waitUntilNav2Active()
|
||||
rclpy.spin(navigator)
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,40 @@
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
navigator = BasicNavigator()
|
||||
# 等待导航启动完成
|
||||
navigator.waitUntilNav2Active()
|
||||
# 设置目标点坐标
|
||||
goal_pose = PoseStamped()
|
||||
goal_pose.header.frame_id = 'map'
|
||||
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose.pose.position.x = 1.0
|
||||
goal_pose.pose.position.y = 1.0
|
||||
goal_pose.pose.orientation.w = 1.0
|
||||
# 发送目标接收反馈结果
|
||||
navigator.goToPose(goal_pose)
|
||||
while not navigator.isTaskComplete():
|
||||
feedback = navigator.getFeedback()
|
||||
navigator.get_logger().info(
|
||||
f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达')
|
||||
# 超时自动取消
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
|
||||
navigator.cancelTask()
|
||||
# 最终结果判断
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
navigator.get_logger().info('导航结果：成功')
|
||||
elif result == TaskResult.CANCELED:
|
||||
navigator.get_logger().warn('导航结果：被取消')
|
||||
elif result == TaskResult.FAILED:
|
||||
navigator.get_logger().error('导航结果：失败')
|
||||
else:
|
||||
navigator.get_logger().error('导航结果：返回状态无效')
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,52 @@
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
navigator = BasicNavigator()
|
||||
navigator.waitUntilNav2Active()
|
||||
# 创建点集
|
||||
goal_poses = []
|
||||
goal_pose1 = PoseStamped()
|
||||
goal_pose1.header.frame_id = 'map'
|
||||
goal_pose1.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose1.pose.position.x = 0.0
|
||||
goal_pose1.pose.position.y = 0.0
|
||||
goal_pose1.pose.orientation.w = 1.0
|
||||
goal_poses.append(goal_pose1)
|
||||
goal_pose2 = PoseStamped()
|
||||
goal_pose2.header.frame_id = 'map'
|
||||
goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose2.pose.position.x = 2.0
|
||||
goal_pose2.pose.position.y = 0.0
|
||||
goal_pose2.pose.orientation.w = 1.0
|
||||
goal_poses.append(goal_pose2)
|
||||
goal_pose3 = PoseStamped()
|
||||
goal_pose3.header.frame_id = 'map'
|
||||
goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose3.pose.position.x = 2.0
|
||||
goal_pose3.pose.position.y = 2.0
|
||||
goal_pose3.pose.orientation.w = 1.0
|
||||
goal_poses.append(goal_pose3)
|
||||
# 调用路点导航服务
|
||||
navigator.followWaypoints(goal_poses)
|
||||
# 判断结束及获取反馈
|
||||
while not navigator.isTaskComplete():
|
||||
feedback = navigator.getFeedback()
|
||||
navigator.get_logger().info(
|
||||
f'当前目标编号：{feedback.current_waypoint}')
|
||||
# 最终结果判断
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
navigator.get_logger().info('导航结果：成功')
|
||||
elif result == TaskResult.CANCELED:
|
||||
navigator.get_logger().warn('导航结果：被取消')
|
||||
elif result == TaskResult.FAILED:
|
||||
navigator.get_logger().error('导航结果：失败')
|
||||
else:
|
||||
navigator.get_logger().error('导航结果：返回状态无效')
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
18
chapt7/chapt7_ws/src/fishbot_application/package.xml
Normal file
18
chapt7/chapt7_ws/src/fishbot_application/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fishbot_application</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
4
chapt7/chapt7_ws/src/fishbot_application/setup.cfg
Normal file
4
chapt7/chapt7_ws/src/fishbot_application/setup.cfg
Normal file
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/fishbot_application
|
||||
[install]
|
||||
install_scripts=$base/lib/fishbot_application
|
||||
26
chapt7/chapt7_ws/src/fishbot_application/setup.py
Normal file
26
chapt7/chapt7_ws/src/fishbot_application/setup.py
Normal file
@@ -0,0 +1,26 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'fishbot_application'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='fishros',
|
||||
maintainer_email='87068644+fishros@users.noreply.github.com',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'init_robot_pose=fishbot_application.init_robot_pose:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
25
chapt7/chapt7_ws/src/fishbot_application/test/test_flake8.py
Normal file
25
chapt7/chapt7_ws/src/fishbot_application/test/test_flake8.py
Normal file
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
23
chapt7/chapt7_ws/src/fishbot_application/test/test_pep257.py
Normal file
23
chapt7/chapt7_ws/src/fishbot_application/test/test_pep257.py
Normal file
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
36
chapt7/chapt7_ws/src/fishbot_application_cpp/CMakeLists.txt
Normal file
36
chapt7/chapt7_ws/src/fishbot_application_cpp/CMakeLists.txt
Normal file
@@ -0,0 +1,36 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(fishbot_application_cpp)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
|
||||
add_executable(nav2pose src/nav2pose.cpp)
|
||||
ament_target_dependencies(nav2pose rclcpp nav2_msgs rclcpp_action)
|
||||
|
||||
install(TARGETS
|
||||
nav2pose
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
18
chapt7/chapt7_ws/src/fishbot_application_cpp/package.xml
Normal file
18
chapt7/chapt7_ws/src/fishbot_application_cpp/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fishbot_application_cpp</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="fish@fishros.com">fishros</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,71 @@
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp" // 导入导航动作消息的头文件
|
||||
#include "rclcpp/rclcpp.hpp" // 导入ROS 2的C++客户端库
|
||||
#include "rclcpp_action/rclcpp_action.hpp" // 导入ROS 2的C++ Action客户端库
|
||||
|
||||
using NavigationAction = nav2_msgs::action::NavigateToPose; // 定义导航动作类型为NavigateToPose
|
||||
|
||||
class NavToPoseClient : public rclcpp::Node {
|
||||
public:
|
||||
using NavigationActionClient = rclcpp_action::Client<NavigationAction>; // 定义导航动作客户端类型
|
||||
using NavigationActionGoalHandle =
|
||||
rclcpp_action::ClientGoalHandle<NavigationAction>; // 定义导航动作目标句柄类型
|
||||
|
||||
NavToPoseClient() : Node("nav_to_pose_client") {
|
||||
// 创建导航动作客户端
|
||||
action_client_ = rclcpp_action::create_client<NavigationAction>(
|
||||
this, "navigate_to_pose");
|
||||
}
|
||||
|
||||
void sendGoal() {
|
||||
// 等待导航动作服务器上线，等待时间为5秒
|
||||
while (!action_client_->wait_for_action_server(std::chrono::seconds(5))) {
|
||||
RCLCPP_INFO(get_logger(), "等待Action服务上线。");
|
||||
}
|
||||
// 设置导航目标点
|
||||
auto goal_msg = NavigationAction::Goal();
|
||||
goal_msg.pose.header.frame_id = "map"; // 设置目标点的坐标系为地图坐标系
|
||||
goal_msg.pose.pose.position.x = 2.0f; // 设置目标点的x坐标为2.0
|
||||
goal_msg.pose.pose.position.y = 2.0f; // 设置目标点的y坐标为2.0
|
||||
|
||||
auto send_goal_options =
|
||||
rclcpp_action::Client<NavigationAction>::SendGoalOptions();
|
||||
// 设置请求目标结果回调函数
|
||||
send_goal_options.goal_response_callback =
|
||||
[this](NavigationActionGoalHandle::SharedPtr goal_handle) {
|
||||
if (goal_handle) {
|
||||
RCLCPP_INFO(get_logger(), "目标点已被服务器接收");
|
||||
}
|
||||
};
|
||||
// 设置移动过程反馈回调函数
|
||||
send_goal_options.feedback_callback =
|
||||
[this](
|
||||
NavigationActionGoalHandle::SharedPtr goal_handle,
|
||||
const std::shared_ptr<const NavigationAction::Feedback> feedback) {
|
||||
(void)goal_handle; // 假装调用，避免 warning: unused
|
||||
RCLCPP_INFO(this->get_logger(), "反馈剩余距离:%f",
|
||||
feedback->distance_remaining);
|
||||
};
|
||||
// 设置执行结果回调函数
|
||||
send_goal_options.result_callback =
|
||||
[this](const NavigationActionGoalHandle::WrappedResult& result) {
|
||||
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||
RCLCPP_INFO(this->get_logger(), "处理成功！");
|
||||
}
|
||||
};
|
||||
// 发送导航目标点
|
||||
action_client_->async_send_goal(goal_msg, send_goal_options);
|
||||
}
|
||||
|
||||
NavigationActionClient::SharedPtr action_client_;
|
||||
};
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<NavToPoseClient>();
|
||||
node->sendGoal();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
32
chapt7/chapt7_ws/src/fishbot_description/CMakeLists.txt
Normal file
32
chapt7/chapt7_ws/src/fishbot_description/CMakeLists.txt
Normal file
@@ -0,0 +1,32 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(fishbot_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
|
||||
install(DIRECTORY launch urdf config world
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
ament_package()
|
||||
202
chapt7/chapt7_ws/src/fishbot_description/LICENSE
Normal file
202
chapt7/chapt7_ws/src/fishbot_description/LICENSE
Normal file
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@@ -0,0 +1,52 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
use_sim_time: true
|
||||
fishbot_joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
use_sim_time: true
|
||||
fishbot_effort_controller:
|
||||
type: effort_controllers/JointGroupEffortController
|
||||
fishbot_diff_drive_controller:
|
||||
type: diff_drive_controller/DiffDriveController
|
||||
|
||||
fishbot_effort_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_wheel_joint
|
||||
- right_wheel_joint
|
||||
command_interfaces:
|
||||
- effort
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
- effort
|
||||
|
||||
|
||||
|
||||
fishbot_diff_drive_controller:
|
||||
ros__parameters:
|
||||
left_wheel_names: ["left_wheel_joint"]
|
||||
right_wheel_names: ["right_wheel_joint"]
|
||||
|
||||
wheel_separation: 0.17
|
||||
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
|
||||
wheel_radius: 0.032
|
||||
|
||||
wheel_separation_multiplier: 1.0
|
||||
left_wheel_radius_multiplier: 1.0
|
||||
right_wheel_radius_multiplier: 1.0
|
||||
|
||||
publish_rate: 50.0
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_footprint
|
||||
pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
|
||||
twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
|
||||
|
||||
open_loop: true
|
||||
enable_odom_tf: true
|
||||
|
||||
cmd_vel_timeout: 0.5
|
||||
#publish_limited_velocity: true
|
||||
use_stamped_vel: false
|
||||
#velocity_rolling_window_size: 10
|
||||
@@ -0,0 +1,170 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 480
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
imu_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 0.4101419746875763
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 777
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000026bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000026b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000026b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d90000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1354
|
||||
X: 70
|
||||
Y: 27
|
||||
@@ -0,0 +1,42 @@
|
||||
import launch
|
||||
import launch_ros
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取默认路径
|
||||
urdf_tutorial_path = get_package_share_directory('fishbot_description')
|
||||
default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf'
|
||||
default_rviz_config_path = urdf_tutorial_path + '/config/rviz/display_model.rviz'
|
||||
# 为 Launch 声明参数
|
||||
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
|
||||
name='model', default_value=str(default_model_path),
|
||||
description='URDF 的绝对路径')
|
||||
# 获取文件内容生成新的参数
|
||||
robot_description = launch_ros.parameter_descriptions.ParameterValue(
|
||||
launch.substitutions.Command(
|
||||
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
|
||||
value_type=str)
|
||||
# 状态发布节点
|
||||
robot_state_publisher_node = launch_ros.actions.Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description}]
|
||||
)
|
||||
# 关节状态发布节点
|
||||
joint_state_publisher_node = launch_ros.actions.Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
)
|
||||
# RViz 节点
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
arguments=['-d', default_rviz_config_path]
|
||||
)
|
||||
return launch.LaunchDescription([
|
||||
action_declare_arg_mode_path,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
rviz_node
|
||||
])
|
||||
@@ -0,0 +1,75 @@
|
||||
import launch
|
||||
import launch_ros
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取默认路径
|
||||
robot_name_in_model = "fishbot"
|
||||
urdf_tutorial_path = get_package_share_directory('fishbot_description')
|
||||
default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro'
|
||||
default_world_path = urdf_tutorial_path + '/world/custom_room.world'
|
||||
# 为 Launch 声明参数
|
||||
action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
|
||||
name='model', default_value=str(default_model_path),
|
||||
description='URDF 的绝对路径')
|
||||
# 获取文件内容生成新的参数
|
||||
robot_description = launch_ros.parameter_descriptions.ParameterValue(
|
||||
launch.substitutions.Command(
|
||||
['xacro ', launch.substitutions.LaunchConfiguration('model')]),
|
||||
value_type=str)
|
||||
|
||||
robot_state_publisher_node = launch_ros.actions.Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description}]
|
||||
)
|
||||
|
||||
# 通过 IncludeLaunchDescription 包含另外一个 launch 文件
|
||||
launch_gazebo = launch.actions.IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([get_package_share_directory(
|
||||
'gazebo_ros'), '/launch', '/gazebo.launch.py']),
|
||||
# 传递参数
|
||||
launch_arguments=[('world', default_world_path),('verbose','true')]
|
||||
)
|
||||
# 请求 Gazebo 加载机器人
|
||||
spawn_entity_node = launch_ros.actions.Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', '/robot_description',
|
||||
'-entity', robot_name_in_model, ])
|
||||
|
||||
# 加载并激活 fishbot_joint_state_broadcaster 控制器
|
||||
load_joint_state_controller = launch.actions.ExecuteProcess(
|
||||
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
|
||||
'fishbot_joint_state_broadcaster'],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
# 加载并激活 fishbot_effort_controller 控制器
|
||||
load_fishbot_effort_controller = launch.actions.ExecuteProcess(
|
||||
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'],
|
||||
output='screen')
|
||||
|
||||
load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess(
|
||||
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'],
|
||||
output='screen')
|
||||
|
||||
return launch.LaunchDescription([
|
||||
action_declare_arg_mode_path,
|
||||
robot_state_publisher_node,
|
||||
launch_gazebo,
|
||||
spawn_entity_node,
|
||||
# 事件动作，当加载机器人结束后执行
|
||||
launch.actions.RegisterEventHandler(
|
||||
event_handler=launch.event_handlers.OnProcessExit(
|
||||
target_action=spawn_entity_node,
|
||||
on_exit=[load_joint_state_controller],)
|
||||
),
|
||||
# 事件动作，load_fishbot_diff_drive_controller
|
||||
launch.actions.RegisterEventHandler(
|
||||
event_handler=launch.event_handlers.OnProcessExit(
|
||||
target_action=load_joint_state_controller,
|
||||
on_exit=[load_fishbot_diff_drive_controller],)
|
||||
),
|
||||
])
|
||||
18
chapt7/chapt7_ws/src/fishbot_description/package.xml
Normal file
18
chapt7/chapt7_ws/src/fishbot_description/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fishbot_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="mzebra@foxmail.com">mzebra</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,8 @@
|
||||
digraph G {
|
||||
node [shape=box];
|
||||
"base_link" [label="base_link"];
|
||||
"imu_link" [label="imu_link"];
|
||||
node [shape=ellipse, color=blue, fontcolor=blue];
|
||||
"base_link" -> "imu_joint" [label="xyz: 0 0 0.03 \nrpy: 0 -0 0"]
|
||||
"imu_joint" -> "imu_link"
|
||||
}
|
||||
BIN
chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.pdf
Normal file
BIN
chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.pdf
Normal file
Binary file not shown.
@@ -0,0 +1,44 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="first_robot">
|
||||
<!-- 机器人身体部分 -->
|
||||
<link name="base_link">
|
||||
<!-- 部件外观描述 -->
|
||||
<visual>
|
||||
<!-- 沿自己几何中心的偏移与旋转量 -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<!-- 几何形状 -->
|
||||
<geometry>
|
||||
<!-- 圆柱体，半径0.1m，高度 0.12m -->
|
||||
<cylinder length="0.12" radius="0.10" />
|
||||
</geometry>
|
||||
<!-- 材质子标签-蓝色 -->
|
||||
<material name="blue">
|
||||
<color rgba="0.1 0.1 1.0 0.5" />
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<!-- 机器人IMU部件 -->
|
||||
<link name="imu_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.02 0.02" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 0.5" />
|
||||
</material>
|
||||
</link>
|
||||
|
||||
<!-- 机器人关节 -->
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<!-- 父部件 -->
|
||||
<parent link="base_link" />
|
||||
<!-- 子部件 -->
|
||||
<child link="imu_link" />
|
||||
<!-- 子部件相对父部件的平移和旋转 -->
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,42 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
|
||||
<!-- 声明 base 模块 -->
|
||||
<xacro:macro name="base_link" params="length radius">
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="${length}" radius="${length}" />
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0.1 0.1 1.0 0.5" />
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</xacro:macro>
|
||||
<!-- 声明 IMU 模块 -->
|
||||
<xacro:macro name="imu_link" params="imu_name xyz">
|
||||
<link name="${imu_name}_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.02 0.02" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 0.5" />
|
||||
</material>
|
||||
</link>
|
||||
<joint name="${imu_name}_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="${imu_name}_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
<!-- 传递参数调用 base_link 模块 -->
|
||||
<xacro:base_link length="0.12" radius="0.1" />
|
||||
<!-- 传递参数调用 imu 模块 -->
|
||||
<xacro:imu_link imu_name="imu_up" xyz="0 0 0.02" />
|
||||
<xacro:imu_link imu_name="imu_down" xyz="0 0 -0.02" />
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="caster_xacro" params="caster_name xyz">
|
||||
<link name="${caster_name}_caster_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<sphere radius="0.016" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<sphere radius="0.016" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:sphere_inertia m="0.01" r="0.016" />
|
||||
</link>
|
||||
<gazebo reference="${caster_name}_caster_link">
|
||||
<mu1 value="0.0" />
|
||||
<mu2 value="0.0" />
|
||||
<kp value="1000000000.0" />
|
||||
<kd value="1.0" />
|
||||
</gazebo>
|
||||
|
||||
<joint name="${caster_name}_caster_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="${caster_name}_caster_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
<axis xyz="0 0 0" />
|
||||
</joint>
|
||||
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,40 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="wheel_xacro" params="wheel_name xyz">
|
||||
<link name="${wheel_name}_wheel_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="1.57079 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.032" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="1.57079 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.04" radius="0.032" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:cylinder_inertia m="0.1" h="0.04" r="0.032"/>
|
||||
</link>
|
||||
<gazebo reference="${wheel_name}_wheel_link">
|
||||
<mu1 value="20.0" />
|
||||
<mu2 value="20.0" />
|
||||
<kp value="1000000000.0" />
|
||||
<kd value="1.0" />
|
||||
</gazebo>
|
||||
|
||||
<joint name="${wheel_name}_wheel_joint" type="continuous">
|
||||
<parent link="base_link" />
|
||||
<child link="${wheel_name}_wheel_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="base_xacro" params="length radius">
|
||||
<link name="base_footprint" />
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0.0 0.0 ${length/2.0+0.032-0.001}" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="${length}" radius="${length}" />
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0.1 0.1 1.0 0.5"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="${length}" radius="${length}" />
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0.1 0.1 1.0 0.5"/>
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}"/>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<xacro:macro name="box_inertia" params="m w h d">
|
||||
<inertial>
|
||||
<mass value="${m}" />
|
||||
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="cylinder_inertia" params="m r h">
|
||||
<inertial>
|
||||
<mass value="${m}" />
|
||||
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="sphere_inertia" params="m r">
|
||||
<inertial>
|
||||
<mass value="${m}" />
|
||||
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,45 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="fishbot_ros2_control">
|
||||
<ros2_control name="FishBotGazeboSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
<joint name="left_wheel_joint">
|
||||
<command_interface name="velocity">
|
||||
<param name="min">-1</param>
|
||||
<param name="max">1</param>
|
||||
</command_interface>
|
||||
<command_interface name="effort">
|
||||
<param name="min">-0.1</param>
|
||||
<param name="max">0.1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
<joint name="right_wheel_joint">
|
||||
<command_interface name="velocity">
|
||||
<param name="min">-1</param>
|
||||
<param name="max">1</param>
|
||||
</command_interface>
|
||||
<command_interface name="effort">
|
||||
<param name="min">-0.1</param>
|
||||
<param name="max">0.1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
<state_interface name="effort" />
|
||||
</joint>
|
||||
</ros2_control>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
|
||||
<parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
|
||||
<ros>
|
||||
<remapping>/fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
|
||||
<remapping>/fishbot_diff_drive_controller/odom:=/odom</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro" />
|
||||
<!-- 传感器组件 -->
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro" />
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro" />
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro" />
|
||||
<!-- 执行器组件 -->
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro" />
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro" />
|
||||
|
||||
<xacro:base_xacro length="0.12" radius="0.1" />
|
||||
<!-- 传感器 -->
|
||||
<xacro:imu_xacro xyz="0 0 0.02" />
|
||||
<xacro:laser_xacro xyz="0 0 0.10" />
|
||||
<xacro:camera_xacro xyz="0.10 0 0.075" />
|
||||
<!-- 执行器主动轮+从动轮 -->
|
||||
<xacro:wheel_xacro wheel_name="left" xyz="0 0.10 -0.06" />
|
||||
<xacro:wheel_xacro wheel_name="right" xyz="0 -0.10 -0.06" />
|
||||
<xacro:caster_xacro caster_name="front" xyz="0.08 0.0 -0.076" />
|
||||
<xacro:caster_xacro caster_name="back" xyz="-0.08 0.0 -0.076" />
|
||||
<!-- Gazebo 插件 -->
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro" />
|
||||
<!-- <xacro:gazebo_control_plugin /> -->
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/fishbot.ros2_control.xacro" />
|
||||
<xacro:fishbot_ros2_control />
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro" />
|
||||
<xacro:gazebo_sensor_plugin />
|
||||
</robot>
|
||||
@@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="gazebo_control_plugin">
|
||||
<gazebo>
|
||||
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<remapping>cmd_vel:=cmd_vel</remapping>
|
||||
<remapping>odom:=odom</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<!-- wheels -->
|
||||
<left_joint>left_wheel_joint</left_joint>
|
||||
<right_joint>right_wheel_joint</right_joint>
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.2</wheel_separation>
|
||||
<wheel_diameter>0.064</wheel_diameter>
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>true</publish_wheel_tf>
|
||||
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<robot_base_frame>base_footprint</robot_base_frame>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,146 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="gazebo_sensor_plugin">
|
||||
<gazebo reference="laser_link">
|
||||
<sensor name="laserscan" type="ray">
|
||||
<plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>laser_link</frame_name>
|
||||
</plugin>
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<update_rate>5</update_rate>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<!-- 激光传感器配置 -->
|
||||
<ray>
|
||||
<!-- 设置扫描范围 -->
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1.000000</resolution>
|
||||
<min_angle>0.000000</min_angle>
|
||||
<max_angle>6.280000</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<!-- 设置扫描距离 -->
|
||||
<range>
|
||||
<min>0.120000</min>
|
||||
<max>8.0</max>
|
||||
<resolution>0.015000</resolution>
|
||||
</range>
|
||||
<!-- 设置噪声 -->
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
|
||||
|
||||
<gazebo reference="imu_link">
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
</plugin>
|
||||
<update_rate>100</update_rate>
|
||||
<always_on>true</always_on>
|
||||
<!-- 六轴噪声设置 -->
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
<gazebo reference="camera_link">
|
||||
<sensor type="depth" name="camera_sensor">
|
||||
<plugin name="depth_camera" filename="libgazebo_ros_camera.so">
|
||||
<frame_name>camera_optical_link</frame_name>
|
||||
</plugin>
|
||||
<always_on>true</always_on>
|
||||
<update_rate>10</update_rate>
|
||||
<camera name="camera">
|
||||
<horizontal_fov>1.5009831567</horizontal_fov>
|
||||
<image>
|
||||
<width>800</width>
|
||||
<height>600</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<distortion>
|
||||
<k1>0.0</k1>
|
||||
<k2>0.0</k2>
|
||||
<k3>0.0</k3>
|
||||
<p1>0.0</p1>
|
||||
<p2>0.0</p2>
|
||||
<center>0.5 0.5</center>
|
||||
</distortion>
|
||||
</camera>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="camera_xacro" params="xyz">
|
||||
<!-- ============相机模块================ -->
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.10 0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.10 0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8"/>
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:box_inertia m="0.01" w="0.02" h="0.10" d="0.02" />
|
||||
</link>
|
||||
<link name="camera_optical_link"></link>
|
||||
<joint name="camera_optical_joint" type="fixed">
|
||||
<parent link="camera_link" />
|
||||
<child link="camera_optical_link" />
|
||||
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="camera_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,33 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="imu_xacro" params="xyz">
|
||||
<link name="imu_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 00" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.02 0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 0.8" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 00" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.02 0.02 0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 0.8" />
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:box_inertia m="0.01" w="0.02" d="0.02" h="0.02"/>
|
||||
</link>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="imu_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,73 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertia.xacro" />
|
||||
<xacro:macro name="laser_xacro" params="xyz">
|
||||
<gazebo reference="laser_cylinder_link">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<gazebo reference="laser_link">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
|
||||
<!-- ============雷达支撑杆================ -->
|
||||
<link name="laser_cylinder_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.10" radius="0.01" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.10" radius="0.01" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8" />
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:cylinder_inertia m="0.01" r="0.01" h="0.10" />
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<joint name="laser_cylinder_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="laser_cylinder_link" />
|
||||
<origin xyz="${xyz}" />
|
||||
</joint>
|
||||
<!-- ============雷达================ -->
|
||||
<link name="laser_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.02" radius="0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<cylinder length="0.02" radius="0.02" />
|
||||
</geometry>
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 0.8" />
|
||||
</material>
|
||||
</collision>
|
||||
<xacro:cylinder_inertia m="0.03" r="0.02" h="0.02" />
|
||||
</link>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<parent link="laser_cylinder_link" />
|
||||
<child link="laser_link" />
|
||||
<origin xyz="0 0 0.05" />
|
||||
</joint>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
2745
chapt7/chapt7_ws/src/fishbot_description/world/custom_room.world
Normal file
2745
chapt7/chapt7_ws/src/fishbot_description/world/custom_room.world
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" ?>
|
||||
<model>
|
||||
<name>room</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.7">model.sdf</sdf>
|
||||
<author>
|
||||
<name></name>
|
||||
<email></email>
|
||||
</author>
|
||||
<description></description>
|
||||
</model>
|
||||
297
chapt7/chapt7_ws/src/fishbot_description/world/room/model.sdf
Normal file
297
chapt7/chapt7_ws/src/fishbot_description/world/room/model.sdf
Normal file
@@ -0,0 +1,297 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.7'>
|
||||
<model name='room'>
|
||||
<pose>-0.972666 -0.903877 0 0 -0 0</pose>
|
||||
<link name='Wall_66'>
|
||||
<collision name='Wall_66_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>19 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_66_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>19 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>0.013921 5.50955 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_67'>
|
||||
<collision name='Wall_67_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>11.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_67_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>11.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>9.43892 -0.040455 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_68'>
|
||||
<collision name='Wall_68_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>19 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_68_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>19 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>0.013921 -5.59046 0 0 -0 3.14159</pose>
|
||||
</link>
|
||||
<link name='Wall_69'>
|
||||
<collision name='Wall_69_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>11.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_69_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>11.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-9.41108 -0.040455 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_71'>
|
||||
<collision name='Wall_71_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>4.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_71_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>4.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-1.11769 3.29046 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_72'>
|
||||
<collision name='Wall_72_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>6.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_72_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>6.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-4.41769 0.990456 0 0 -0 3.14159</pose>
|
||||
</link>
|
||||
<link name='Wall_79'>
|
||||
<collision name='Wall_79_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.47722 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_79_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>3.47722 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>0.391784 -3.86799 0 0 -0 1.5708</pose>
|
||||
</link>
|
||||
<link name='Wall_80'>
|
||||
<collision name='Wall_80_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>7.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_80_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>7.5 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>4.06678 -2.24484 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_85'>
|
||||
<collision name='Wall_85_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>6.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_85_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>6.25 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-6.38892 -1.80969 0 0 -0 0</pose>
|
||||
</link>
|
||||
<link name='Wall_86'>
|
||||
<collision name='Wall_86_Collision'>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
</collision>
|
||||
<visual name='Wall_86_Visual'>
|
||||
<pose>0 0 1.25 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>2.75 0.15 2.5</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
</material>
|
||||
<meta>
|
||||
<layer>0</layer>
|
||||
</meta>
|
||||
</visual>
|
||||
<pose>-3.33892 -3.10969 0 0 -0 -1.5708</pose>
|
||||
</link>
|
||||
<static>1</static>
|
||||
</model>
|
||||
</sdf>
|
||||
31
chapt7/chapt7_ws/src/fishbot_navigation2/CMakeLists.txt
Normal file
31
chapt7/chapt7_ws/src/fishbot_navigation2/CMakeLists.txt
Normal file
@@ -0,0 +1,31 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(fishbot_navigation2)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(DIRECTORY launch maps config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
ament_package()
|
||||
349
chapt7/chapt7_ws/src/fishbot_navigation2/config/nav2_params.yaml
Normal file
349
chapt7/chapt7_ws/src/fishbot_navigation2/config/nav2_params.yaml
Normal file
@@ -0,0 +1,349 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_footprint"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,49 @@
|
||||
import os
|
||||
import launch
|
||||
import launch_ros
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 获取与拼接默认路径
|
||||
fishbot_navigation2_dir = get_package_share_directory(
|
||||
'fishbot_navigation2')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
rviz_config_dir = os.path.join(
|
||||
nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
|
||||
|
||||
# 创建 Launch 配置
|
||||
use_sim_time = launch.substitutions.LaunchConfiguration(
|
||||
'use_sim_time', default='true')
|
||||
map_yaml_path = launch.substitutions.LaunchConfiguration(
|
||||
'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
|
||||
nav2_param_path = launch.substitutions.LaunchConfiguration(
|
||||
'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))
|
||||
|
||||
return launch.LaunchDescription([
|
||||
# 声明新的 Launch 参数
|
||||
launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path,
|
||||
description='Full path to map file to load'),
|
||||
launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
|
||||
description='Full path to param file to load'),
|
||||
|
||||
launch.actions.IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[nav2_bringup_dir, '/launch', '/bringup_launch.py']),
|
||||
# 使用 Launch 参数替换原有参数
|
||||
launch_arguments={
|
||||
'map': map_yaml_path,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_param_path}.items(),
|
||||
),
|
||||
launch_ros.actions.Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_dir],
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
output='screen'),
|
||||
])
|
||||
BIN
chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.pgm
Normal file
BIN
chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.pgm
Normal file
Binary file not shown.
7
chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.yaml
Normal file
7
chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.yaml
Normal file
@@ -0,0 +1,7 @@
|
||||
image: room.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-10.4, -6.53, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
18
chapt7/chapt7_ws/src/fishbot_navigation2/package.xml
Normal file
18
chapt7/chapt7_ws/src/fishbot_navigation2/package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>fishbot_navigation2</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="87068644+fishros@users.noreply.github.com">fishros</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Reference in New Issue
Block a user