HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

created

Browse Source
This commit is contained in:
HiepLM 2025-12-05 11:12:17 +07:00
parent f60cbc2ed3
commit 45d965671e
196 changed files with 41817 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -412,3 +412,4 @@ FodyWeavers.xsd
# Built Visual Studio Code Extensions
*.vsix
build

24
.gitmodules vendored Normal file
View File

@ -0,0 +1,24 @@
[submodule "src/Libraries/common_msgs"]
path = src/Libraries/common_msgs
url = https://git.pnkr.asia/DuongTD/common_msgs.git
[submodule "src/Libraries/costmap_2d"]
path = src/Libraries/costmap_2d
url = https://git.pnkr.asia/DuongTD/costmap_2d.git
[submodule "src/Libraries/geometry2"]
path = src/Libraries/geometry2
url = https://git.pnkr.asia/DuongTD/geometry2.git
[submodule "src/Libraries/xmlrpcpp"]
path = src/Libraries/xmlrpcpp
url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
[submodule "src/Libraries/laser_geometry"]
path = src/Libraries/laser_geometry
url = https://git.pnkr.asia/DuongTD/laser_geometry.git
[submodule "src/Libraries/robot_time"]
path = src/Libraries/robot_time
url = https://git.pnkr.asia/DuongTD/robot_time.git
[submodule "src/Libraries/voxel_grid"]
path = src/Libraries/voxel_grid
url = https://git.pnkr.asia/DuongTD/voxel_grid.git
[submodule "src/Libraries/data_convert"]
path = src/Libraries/data_convert
url = https://git.pnkr.asia/DuongTD/data_convert.git

71
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,71 @@
{
"files.associations": {
"stdexcept": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"bit": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"compare": "cpp",
"complex": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"ranges": "cpp",
"sstream": "cpp",
"stop_token": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
"codecvt": "cpp"
}
}

58
CATKIN_BUILD_README.md Normal file
View File

@ -0,0 +1,58 @@
# Catkin Build Instructions
## Vấn đề
Thư mục này có `CMakeLists.txt` ở root để hỗ trợ **standalone CMake build**.
Tuy nhiên, **catkin workspace không được phép có CMakeLists.txt ở root**.
Khi chạy `catkin_make` trong thư mục này, bạn sẽ gặp lỗi:
```
The specified base path contains a CMakeLists.txt but "catkin_make" must be invoked in the root of workspace
```
## Giải pháp
### Option 1: Sử dụng script tự động (Khuyến nghị)
```bash
# Chạy script setup
./setup_catkin_workspace.sh
# Build trong workspace mới
cd ../pnkx_nav_catkin_ws
catkin_make
source devel/setup.bash
```
### Option 2: Tạo workspace thủ công
```bash
# Tạo workspace
mkdir -p ~/pnkx_nav_catkin_ws/src
cd ~/pnkx_nav_catkin_ws/src
# Link các packages có package.xml
ln -s /path/to/pnkx_nav_core/src/Navigations/Packages/move_base move_base
# ... link các packages khác nếu cần
# Build
cd ~/pnkx_nav_catkin_ws
catkin_make
```
## Build Systems
Project này hỗ trợ **2 build systems**:
1. **Standalone CMake** (mặc định)
- Build trực tiếp trong thư mục này: `mkdir build && cd build && cmake .. && make`
- Không cần catkin workspace
2. **Catkin** (cho ROS integration)
- Cần tạo workspace riêng (xem hướng dẫn trên)
- Packages tự động detect catkin mode khi build trong workspace
## Chi tiết
Xem `src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md` để biết chi tiết về cách build package `move_base` với cả hai hệ thống.

140
CMakeLists.txt Normal file
View File

@ -0,0 +1,140 @@
# ========================================================
# ROOT CMakeLists.txt
# Đặt ở: Navigations/CMakeLists.txt
# ========================================================
cmake_minimum_required(VERSION 3.0.2)
project(navigations VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Build type
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# Flags chung
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
message(STATUS "========================================")
message(STATUS "Building Navigations Project")
message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "========================================")
# Build các packages theo thứ tự phụ thuộc
# 1. Core libraries (header-only hoặc base libraries)
if (NOT TARGET tf3)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3)
endif()
if (NOT TARGET robot_time)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)
endif()
if (NOT TARGET xmlrpcpp)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/xmlrpcpp)
endif()
if (NOT TARGET robot_cpp)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_cpp)
endif()
if (NOT TARGET data_convert)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/data_convert)
endif()
if (NOT TARGET geometry2)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/geometry2)
endif()
if (NOT TARGET common_msgs)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/common_msgs)
endif()
if (NOT TARGET laser_geometry)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_geometry)
endif()
if (NOT TARGET voxel_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid)
endif()
if (NOT TARGET nav_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid)
endif()
if (NOT TARGET nav_2d_msgs)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs)
endif()
if (NOT TARGET costmap_2d)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/costmap_2d)
endif()
if (NOT TARGET nav_2d_utils)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_utils)
endif()
if (NOT TARGET nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
endif()
if (NOT TARGET nav_core2)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
endif()
if (NOT TARGET nav_core_adapter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
endif()
if (NOT TARGET move_base_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/move_base_core)
endif()
if (NOT TARGET mkt_msgs)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs)
endif()
if (NOT TARGET angles)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles)
endif()
if (NOT TARGET kalman)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman)
endif()
if (NOT TARGET score_algorithm)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
endif()
#if (NOT TARGET mkt_algorithm)
# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
#endif()
if (NOT TARGET two_points_planner)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
endif()
# 2. Main packages (phụ thuộc vào cores)
message(STATUS "[move_base] Shared library configured")
if (NOT TARGET move_base)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
endif()
# C API for .NET/C# integration
if (NOT TARGET navigation_c_api)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
endif()
message(STATUS "========================================")
message(STATUS "All packages configured successfully")
message(STATUS "========================================")

48
config/base_local_planner_params.yaml Normal file
View File

@ -0,0 +1,48 @@
#For full documentation of the parameters in this file, and a list of all the
#parameters available for TrajectoryPlannerROS, please see
#http://www.ros.org/wiki/base_local_planner
TrajectoryPlannerROS:
#Set the acceleration limits of the robot
acc_lim_th: 3.2
acc_lim_x: 0.5
acc_lim_y: 0.5
#Set the velocity limits of the robot
max_vel_x: 0.4
min_vel_x: 0.1
max_rotational_vel: 1.0
min_in_place_rotational_vel: 0.4
#The velocity the robot will command when trying to escape from a stuck situation
escape_vel: -0.2
#For this example, we'll use a holonomic robot
holonomic_robot: false
#Since we're using a holonomic robot, we'll set the set of y velocities it will sample
y_vels: [-0.3, -0.1, 0.1, 0.3]
#Set the tolerance on achieving a goal
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.05
#We'll configure how long and with what granularity we'll forward simulate trajectories
sim_time: 3.0
sim_granularity: 0.025
vx_samples: 5
vtheta_samples: 20
#Parameters for scoring trajectories
goal_distance_bias: 0.4
path_distance_bias: 0.7
occdist_scale: 0.3
heading_lookahead: 0.325
#We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
dwa: false
#How far the robot must travel before oscillation flags are reset
oscillation_reset_dist: 0.01
#Eat up the plan as the robot moves along it
prune_plan: false

54
config/costmap_common_params.yaml Executable file
View File

@ -0,0 +1,54 @@
robot_base_frame: base_footprint
transform_tolerance: 1.0
obstacle_range: 3.0
#mark_threshold: 1
publish_voxel_map: true
footprint_padding: 0.0
navigation_map:
map_topic: /map
map_pkg: managerments
map_file: maze
virtual_walls_map:
map_topic: /virtual_walls/map
namespace: /virtual_walls
map_pkg: managerments
map_file: maze
use_maximum: true
lethal_cost_threshold: 100
obstacles:
observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
f_scan_marking:
topic: /f_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
f_scan_clearing:
topic: /f_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_marking:
topic: /b_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_clearing:
topic: /b_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25

13
config/costmap_global_params.yaml Executable file
View File

@ -0,0 +1,13 @@
global_costmap:
global_frame: map
update_frequency: 1.0
publish_frequency: 1.0
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.2
rolling_window: false
z_voxels: 10
inflation:
cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.

10
config/costmap_global_params_plugins_no_virtual_walls.yaml Executable file
View File

@ -0,0 +1,10 @@
global_costmap:
frame_id: map
plugins:
- {name: navigation_map, type: "costmap_2d::StaticLayer" }
- {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles:
enabled: false
footprint_clearing_enabled: false

16
config/costmap_local_params.yaml Executable file
View File

@ -0,0 +1,16 @@
local_costmap:
global_frame: odom
update_frequency: 6.0
publish_frequency: 6.0
rolling_window: true
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.15
z_voxels: 8
inflation:
cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
width: 8.0
height: 8.0
origin_x: 0.0
origin_y: 0.0

8
config/costmap_local_params_plugins_no_virtual_walls.yaml Executable file
View File

@ -0,0 +1,8 @@
local_costmap:
frame_id: odom
plugins:
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles:
enabled: true
footprint_clearing_enabled: true

17
config/custom_global_params.yaml Normal file
View File

@ -0,0 +1,17 @@
base_global_planner: CustomPlanner
CustomPlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: false
nominalvel_mpersecs: 0.8
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
allow_unknown: true
directory_to_save_paths: "/init/paths"
pathway_filename: "pathway.txt"
current_pose_topic_name: "/amcl_pose"
map_frame_id: "map"
base_frame_id: "base_link"

55
config/dwa_local_planner_params.yaml Executable file
View File

@ -0,0 +1,55 @@
base_local_planner: dwa_local_planner/DWAPlannerROS
DWAPlannerROS:
# Robot configuration
max_vel_x: 0.8
min_vel_x: -0.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_trans_vel: 0.8 # choose slightly less than the base's capability
min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.03
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_rot_vel: 1.0 # choose slightly less than the base's capability
min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # diff drive robot
acc_limit_trans: 1.5
acc_lim_theta: 2.0
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
latch_xy_goal_tolerance: true
# Forward simulation
sim_time: 1.2
vx_samples: 15
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 20
# Trajectory scoring
path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal
occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj.
scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed.
prune_plan: true
# Oscillation prevention
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m
oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: /odom # or <robot namespace>/odom

217
config/ekf.yaml Executable file
View File

@ -0,0 +1,217 @@
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 50
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: true
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified
base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified
world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
odom0_config: [false, false, false, # x y z
false, false, false, # roll pitch yaw
true, true, false, # vx vy vz
false, false, true, # vroll vpitch vyaw
false, false, false] # ax ay az
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 10
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
#odom0_pose_rejection_threshold: 5
#odom0_twist_rejection_threshold: 1
# Further input parameter examples
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
imu0: imu_data
imu0_config: [false, false, false, # x y z
false, false, true, # roll pitch yaw
false, false, false, # vx vy vz
false, false, true, # vroll vpitch vyaw
true, false, false] # ax ay az
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
#imu0_twist_rejection_threshold: 0.8 #
#imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: false
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: false
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

36
config/localization.yaml Normal file
View File

@ -0,0 +1,36 @@
Amcl:
use_map_topic: true
odom_model_type: "diff-corrected"
gui_publish_rate: 5.0
save_pose_rate: 0.5
laser_max_beams: 300
laser_min_range: -1.0
laser_max_range: -1.0
min_particles: 1000
max_particles: 3000
kld_err: 0.05
kld_z: 0.99
odom_alpha1: 0.02
odom_alpha2: 0.01
odom_alpha3: 0.01
odom_alpha4: 0.02
laser_z_hit: 0.5
laser_z_short: 0.05
laser_z_max: 0.05
laser_z_rand: 0.5
laser_sigma_hit: 0.2
laser_lambda_short: 0.1
laser_model_type: "likelihood_field"
laser_likelihood_max_dist: 1.0
update_min_d: 0.05
update_min_a: 0.05
odom_frame_id: odom
base_frame_id: base_footprint
global_frame_id: map
resample_interval: 1
transform_tolerance: 0.2
recovery_alpha_slow: 0.001
recovery_alpha_fast: 0.001
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0

152
config/maker_sources.yaml Normal file
View File

@ -0,0 +1,152 @@
maker_sources: trolley charger dock_station undock_station dock_station_2 undock_station_2
trolley:
plugins:
- {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""}
# - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
4legs:
maker_goal_frame: trolley_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.02
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
charger:
plugins:
- {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
charger:
maker_goal_frame: charger_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.1
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
dock_station:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
station:
maker_goal_frame: dock_station_goal
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.4
dock_station_2:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
station:
maker_goal_frame: dock_station_goal_2
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.4
undock_station:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
station:
maker_goal_frame: undock_station_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.3
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.015
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.03
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
undock_station_2:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
station:
maker_goal_frame: undock_station_goal_2
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.25
vel_theta: 0.5
yaw_goal_tolerance: 0.015
xy_goal_tolerance: 0.03
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16
qrcode:
maker_goal_frame: qr_trolley
delay: 1.5 # Cấm sửa không là không chạy được
timeout: 60.0
vel_x: 0.05
vel_theta: 0.5
allow_rotate: true
yaw_goal_tolerance: 0.02
xy_goal_tolerance: 0.02
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.16

63
config/mapping.yaml Normal file
View File

@ -0,0 +1,63 @@
SlamToolBox:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping #localization
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 10.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 14400.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

24
config/move_base_common_params.yaml Executable file
View File

@ -0,0 +1,24 @@
### replanning
controller_frequency: 20.0 # run controller at 15.0 Hz
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 5 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.4
### recovery behaviors
recovery_behavior_enabled: true
recovery_behaviors: [
{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
]
conservative_reset:
reset_distance: 3.0 # clear obstacles farther away than 3.0 m
invert_area_to_clear: true
aggressive_reset:
reset_distance: 3.0

106
config/mpc_local_planner_params.yaml Executable file
View File

@ -0,0 +1,106 @@
base_local_planner: mpc_local_planner/MpcLocalPlannerROS
MpcLocalPlannerROS:
odom_topic: odom
## Robot settings
robot:
type: "unicycle"
unicycle:
max_vel_x: 0.5
max_vel_x_backwards: 0.3
max_vel_theta: 0.3
acc_lim_x: 0.4 # deactive bounds with zero
dec_lim_x: 0.4 # deactive bounds with zero
acc_lim_theta: 0.6 # deactivate bounds with zero
## Footprint model for collision avoidance
footprint_model:
type: "point"
is_footprint_dynamic: False
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_dist: 0.5
cutoff_dist: 2.0
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
collision_check_no_poses: 5
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
dt_ref: 0.3 # and here the corresponding temporal resolution
xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: False # We want a fixed grid
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
quadratic_form:
state_weights: [2.0, 2.0, 0.25]
control_weights: [0.1, 0.05]
integral_form: False
terminal_cost:
type: "quadratic" # can be "none"
quadratic:
final_state_weights: [10.0, 10.0, 0.5]
terminal_constraint:
type: "none" # can be "none"
l2_ball:
weight_matrix: [1.0, 1.0, 1.0]
radius: 5
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.04
global_plan_overwrite_orientation: False
global_plan_prune_distance: 1.5
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.0 # Check horizon length
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0
prefer_x_feedback: False
publish_ocp_results: True
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-3
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "exact" # exact or limited-memory
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

280
config/mprim/genmprim_unicycle_highcost_5cm.m Executable file
View File

@ -0,0 +1,280 @@
% /*
% * Copyright (c) 2008, Maxim Likhachev
% * All rights reserved.
% *
% * Redistribution and use in source and binary forms, with or without
% * modification, are permitted provided that the following conditions are met:
% *
% * * Redistributions of source code must retain the above copyright
% * notice, this list of conditions and the following disclaimer.
% * * Redistributions in binary form must reproduce the above copyright
% * notice, this list of conditions and the following disclaimer in the
% * documentation and/or other materials provided with the distribution.
% * * Neither the name of the Carnegie Mellon University nor the names of its
% * contributors may be used to endorse or promote products derived from
% * this software without specific prior written permission.
% *
% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% * POSSIBILITY OF SUCH DAMAGE.
% */
function[] = genmprim_unicycle_highcost_5cm(outfilename)
%
%generates motion primitives and saves them into file
%
%written by Maxim Likhachev
%---------------------------------------------------
%
%defines
UNICYCLE_MPRIM_16DEGS = 1;
if UNICYCLE_MPRIM_16DEGS == 1
resolution = 0.05;
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7;
%multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1;
backwardcostmult = 40;
forwardandturncostmult = 2;
sidestepcostmult = 10;
turninplacecostmult = 20;
%note, what is shown x,y,theta changes (not absolute numbers)
%0 degreees
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
%1/16 theta change
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
%turn in place
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
%45 degrees
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
%turn in place
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
%22.5 degrees
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
%turn in place
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
else
fprintf(1, 'ERROR: undefined mprims type\n');
return;
end;
fout = fopen(outfilename, 'w');
%write the header
fprintf(fout, 'resolution_m: %f\n', resolution);
fprintf(fout, 'numberofangles: %d\n', numberofangles);
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
%iterate over angles
for angleind = 1:numberofangles
figure(1);
hold off;
text(0, 0, int2str(angleind));
%iterate over primitives
for primind = 1:numberofprimsperangle
fprintf(fout, 'primID: %d\n', primind-1);
fprintf(fout, 'startangle_c: %d\n', angleind-1);
%current angle
currentangle = (angleind-1)*2*pi/numberofangles;
currentangle_36000int = round((angleind-1)*36000/numberofangles);
%compute which template to use
if (rem(currentangle_36000int, 9000) == 0)
basemprimendpts_c = basemprimendpts0_c(primind,:);
angle = currentangle;
elseif (rem(currentangle_36000int, 4500) == 0)
basemprimendpts_c = basemprimendpts45_c(primind,:);
angle = currentangle - 45*pi/180;
elseif (rem(currentangle_36000int-7875, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
angle = currentangle - 78.75*pi/180;
fprintf(1, '78p75\n');
elseif (rem(currentangle_36000int-6750, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
angle = currentangle - 67.5*pi/180;
fprintf(1, '67p5\n');
elseif (rem(currentangle_36000int-5625, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
angle = currentangle - 56.25*pi/180;
fprintf(1, '56p25\n');
elseif (rem(currentangle_36000int-3375, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
angle = currentangle - 33.75*pi/180;
fprintf(1, '33p75\n');
elseif (rem(currentangle_36000int-2250, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
angle = currentangle - 22.5*pi/180;
fprintf(1, '22p5\n');
elseif (rem(currentangle_36000int-1125, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
angle = currentangle - 11.25*pi/180;
fprintf(1, '11p25\n');
else
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
return;
end;
%now figure out what action will be
baseendpose_c = basemprimendpts_c(1:3);
additionalactioncostmult = basemprimendpts_c(4);
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
endpose_c = [endx_c endy_c endtheta_c];
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
end;
%generate intermediate poses (remember they are w.r.t 0,0 (and not
%centers of the cells)
numofsamples = 10;
intermcells_m = zeros(numofsamples,3);
if UNICYCLE_MPRIM_16DEGS == 1
startpt = [0 0 currentangle];
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
intermcells_m = zeros(numofsamples,3);
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
for iind = 1:numofsamples
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
0];
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
end;
else %unicycle-based move forward or backward
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
l = S(1);
tvoverrv = S(2);
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
tv = tvoverrv*rv;
if l < 0
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
l = 0;
end;
%compute rv
%rv = baseendpose_c(3)*2*pi/numberofangles;
%compute tv
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
%tv = (tvx + tvy)/2.0;
%generate samples
for iind = 1:numofsamples
dt = (iind-1)/(numofsamples-1);
%dtheta = rv*dt + startpt(3);
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
% dtheta];
if(dt*tv < l)
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
startpt(2) + dt*tv*sin(startpt(3)) ...
startpt(3)];
else
dtheta = rv*(dt - l/tv) + startpt(3);
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
dtheta];
end;
end;
%correct
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
endpt(2) - intermcells_m(numofsamples,2)];
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
interpfactor = [0:1/(numofsamples-1):1];
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
end;
end;
%write out
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
for interind = 1:size(intermcells_m, 1)
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
end;
plot(intermcells_m(:,1), intermcells_m(:,2));
axis([-0.3 0.3 -0.3 0.3]);
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
hold on;
end;
grid;
pause;
end;
fclose('all');

416
config/mprim/genmprim_unicycle_highcost_5cm.py Executable file
View File

@ -0,0 +1,416 @@
#!/usr/bin/env python3
#
# Copyright (c) 2016, David Conner (Christopher Newport University)
# Based on genmprim_unicycle.m
# Copyright (c) 2008, Maxim Likhachev
# All rights reserved.
# converted by libermate utility (https://github.com/awesomebytes/libermate)
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Carnegie Mellon University nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import numpy as np
import rospkg
# if available import pylab (from matlibplot)
matplotlib_found = False
try:
import matplotlib.pylab as plt
matplotlib_found = True
except ImportError:
pass
def matrix_size(mat, elem=None):
if not elem:
return mat.shape
else:
return mat.shape[int(elem) - 1]
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
visualize = matplotlib_found and visualize # Plot the primitives
# Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
# baseendpose_c, additionalactioncostmult, fout, numofsamples,
# basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
# numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
# rotation_angle, basemprimendpts_c, forwardandturncostmult,
# forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
# interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
# currentangle, numberofprimsperangle, resolution, currentangle_36000int,
# l, iind, errorxy, interind, endy_c, angleind, endpt
# Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
# int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
# round, size
# %
# %generates motion primitives and saves them into file
# %
# %written by Maxim Likhachev
# %---------------------------------------------------
# %
# %defines
UNICYCLE_MPRIM_16DEGS = 1.0
if UNICYCLE_MPRIM_16DEGS == 1.0:
resolution = 0.05
numberofangles = 16
# %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7
# %multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1.0
backwardcostmult = 40.0
forwardandturncostmult = 2.0
# sidestepcostmult = 10.0
turninplacecostmult = 20.0
# %note, what is shown x,y,theta changes (not absolute numbers)
# %0 degreees
basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %45 degrees
basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %22.5 degrees
basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
else:
print('ERROR: undefined mprims type\n')
return []
fout = open(outfilename, 'w')
# %write the header
fout.write('resolution_m: %f\n' % (resolution))
fout.write('numberofangles: %d\n' % (numberofangles))
fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
# %iterate over angles
for angleind in np.arange(1.0, (numberofangles) + 1):
currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
if visualize:
if separate_plots:
fig = plt.figure(angleind)
plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
else:
fig = plt.figure(1)
plt.axis('equal')
plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
ax = fig.add_subplot(1, 1, 1)
major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
ax.set_xticks(major_ticks)
ax.set_xticks(minor_ticks, minor=True)
ax.set_yticks(major_ticks)
ax.set_yticks(minor_ticks, minor=True)
ax.grid(which='minor', alpha=0.5)
ax.grid(which='major', alpha=0.9)
# %iterate over primitives
for primind in np.arange(1.0, (numberofprimsperangle) + 1):
fout.write('primID: %d\n' % (primind - 1))
fout.write('startangle_c: %d\n' % (angleind - 1))
# %current angle
# %compute which template to use
if (currentangle_36000int % 9000) == 0:
basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
angle = currentangle
elif (currentangle_36000int % 4500) == 0:
basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
angle = currentangle - 45.0 * np.pi / 180.0
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 7875) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts33p75_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (78.75 * np.pi) / 180.0
# print('78p75\n')
elif ((currentangle_36000int - 6750) % 9000) == 0:
basemprimendpts_c = (
1 * basemprimendpts22p5_c[int(primind) - 1, :]
) # 1* to force deep copy to avoid reference update below
basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
# %reverse x and y
basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
# %reverse the angle as well
# print(
# '%d : %d %d %d onto %d %d %d\n'
# % (
# primind - 1,
# basemprimendpts22p5_c[int(primind) - 1, 0],
# basemprimendpts22p5_c[int(primind) - 1, 1],
# basemprimendpts22p5_c[int(primind) - 1, 2],
# basemprimendpts_c[0],
# basemprimendpts_c[1],
# basemprimendpts_c[2],
# )
# )
angle = currentangle - (67.5 * np.pi) / 180.0
print('67p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 5625) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts11p25_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (56.25 * np.pi) / 180.0
# print('56p25\n')
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 3375) % 9000) == 0:
# basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
# angle = currentangle - (33.75 * np.pi) / 180.0
# print('33p75\n')
elif ((currentangle_36000int - 2250) % 9000) == 0:
basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
angle = currentangle - (22.5 * np.pi) / 180.0
print('22p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 1125) % 9000) == 0:
# basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
# angle = currentangle - (11.25 * np.pi) / 180.0
# print('11p25\n')
else:
print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
return []
# %now figure out what action will be
baseendpose_c = basemprimendpts_c[0:3]
additionalactioncostmult = basemprimendpts_c[3]
endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
print("endpose_c=", endpose_c)
print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
# if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
# %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
# %generate intermediate poses (remember they are w.r.t 0,0 (and not
# %centers of the cells)
numofsamples = 10
intermcells_m = np.zeros((numofsamples, 3))
if UNICYCLE_MPRIM_16DEGS == 1.0:
startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
endpt = np.array(
np.hstack(
(
(endpose_c[0] * resolution),
(endpose_c[1] * resolution),
(
((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
/ numberofangles
),
)
)
)
print("startpt =", startpt)
print("endpt =", endpt)
intermcells_m = np.zeros((numofsamples, 3))
if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
# %turn in place or move forward
for iind in np.arange(1.0, (numofsamples) + 1):
fraction = float(iind - 1) / (numofsamples - 1)
intermcells_m[int(iind) - 1, :] = np.array(
(
startpt[0] + (endpt[0] - startpt[0]) * fraction,
startpt[1] + (endpt[1] - startpt[1]) * fraction,
0,
)
)
rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
# print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
else:
# %unicycle-based move forward or backward (http://sbpl.net/node/53)
R = np.array(
np.vstack(
(
np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
)
)
)
S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
l = S[0]
tvoverrv = S[1]
rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
tv = tvoverrv * rv
# print "R=\n",R
# print "Rpi=\n",np.linalg.pinv(R)
# print "S=\n",S
# print "l=",l
# print "tvoverrv=",tvoverrv
# print "rv=",rv
# print "tv=",tv
if l < 0.0:
print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
l = 0.0
# %compute rv
# %rv = baseendpose_c(3)*2*pi/numberofangles;
# %compute tv
# %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
# %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
# %tv = (tvx + tvy)/2.0;
# %generate samples
for iind in np.arange(1, numofsamples + 1):
dt = (iind - 1) / (numofsamples - 1)
# %dtheta = rv*dt + startpt(3);
# %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
# % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
# % dtheta];
if (dt * tv) < l:
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0] + dt * tv * np.cos(startpt[2]),
startpt[1] + dt * tv * np.sin(startpt[2]),
startpt[2],
)
)
)
else:
dtheta = rv * (dt - l / tv) + startpt[2]
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0]
+ l * np.cos(startpt[2])
+ tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
startpt[1]
+ l * np.sin(startpt[2])
- tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
dtheta,
)
)
)
# %correct
errorxy = np.array(
np.hstack(
(
endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
)
)
)
# print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
interpfactor = np.array(
np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
)
# print "intermcells_m=",intermcells_m
# print "interp'=",interpfactor.conj().T
intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
# %write out
fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
fout.write(
'%.4f %.4f %.4f\n'
% (
intermcells_m[int(interind) - 1, 0],
intermcells_m[int(interind) - 1, 1],
intermcells_m[int(interind) - 1, 2],
)
)
if visualize:
plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
# if (visualize):
# plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
fout.close()
if visualize:
# plt.waitforbuttonpress() # hold until buttom pressed
plt.show() # Keep windows open until the program is terminated
return []
if __name__ == "__main__":
rospack = rospkg.RosPack()
outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
genmprim_unicycle(outfilename, visualize=True)

1203
config/mprim/unicycle_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_5cm_expensive_turn_in_place.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

2403
config/mprim/unicycle_5cm_noreverse_trolley.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_highcost_10cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_highcost_1cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_highcost_2_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_highcost_2cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
config/mprim/unicycle_highcost_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

8
config/mqtt_general.yaml Normal file
View File

@ -0,0 +1,8 @@
MQTT:
Name: T800
Host: 172.20.235.170
Port: 1885
Client_ID: T800
Username: robotics
Password: robotics
Keep_Alive: 60

186
config/pnkx_local_planner_params.yaml Normal file
View File

@ -0,0 +1,186 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: nav_core_adapter::LocalPlannerAdapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
stateful: true
publish_topic: true
LocalPlannerAdapter:
PNKXLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXDockingLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXGoStraightLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::GoStraight
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXRotateLocalPlanner:
# Algorithm
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
# Goal checking
goal_checker_name: mkt_plugins::SimpleGoalChecker
stateful: true
LimitedAccelGenerator:
max_vel_x: 1.2
min_vel_x: -1.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability
min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
max_vel_theta: 0.9 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.04 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.0
acc_lim_y: 0.0 # diff drive robot
acc_lim_theta: 0.9
decel_lim_x: -1.0
decel_lim_y: -0.0
decel_lim_theta: -1.5
# Whether to split the path into segments or not
split_path: true
sim_time: 1.5
vx_samples: 15
vy_samples: 1
vtheta_samples: 10
discretize_by_time: true
angular_granularity: 0.05
linear_granularity: 0.1
# sim_period
include_last_point: true
PredictiveTrajectory:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.1
trans_stopped_velocity: 0.1
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
# only when true:
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
regulated_linear_scaling_min_speed: 0.15 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
# Inflation cost scaling (Limit velocity by proximity to obstacles)
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)
GoStraight:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
# stoped
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06
use_regulated_linear_velocity_scaling: false
use_cost_regulated_linear_velocity_scaling: false
RotateToGoal:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
# only when true:
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
# Inflation cost scaling (Limit velocity by proximity to obstacles)
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)

71
config/ros_diff_drive_controller.yaml Executable file
View File

@ -0,0 +1,71 @@
# -----------------------------------
left_wheel : 'left_wheel_joint'
right_wheel : 'right_wheel_joint'
publish_rate: 50 # this is what the real MiR platform publishes (default: 50)
# These covariances are exactly what the real MiR platform publishes
pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
enable_odom_tf: true
enable_wheel_tf: true
allow_multiple_cmd_vel_publishers: true
# open_loop: false
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter.
# We don't set the value here because it's different for each MiR type (100, 250, ...), and
# the plugin figures out the correct values.
wheel_separation : 0.5106
wheel_radius : 0.1
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 1.0
# frame_ids (same as real MiR platform)
base_frame_id: base_footprint # default: base_link base_footprint
odom_frame_id: odom # default: odom
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : true
max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8
min_velocity : -1.5 # m/s
has_acceleration_limits: true
max_acceleration : 0.5 # m/s^2; move_base acc_lim_x: 1.5
min_acceleration : -0.5 # m/s^2
has_jerk_limits : true
max_jerk : 3.0 # m/s^3
angular:
z:
has_velocity_limits : true
max_velocity : 1.0 # rad/s; move_base max_rot_vel: 1.0
min_velocity : -1.0
has_acceleration_limits: true
max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0
has_jerk_limits : true
max_jerk : 2.5 # rad/s^3
left_wheel_joint:
lookup_name: WheelPlugin
max_publish_rate: 50
mesurement_topic: left_encoder
frame_id: left_wheel_link
wheel_topic: /left_wheel
subscribe_queue_size: 1
command_timeout: 5.0
origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id
right_wheel_joint:
lookup_name: WheelPlugin
max_publish_rate: 50
mesurement_topic: right_encoder
frame_id: right_wheel_link
wheel_topic: /right_wheel
subscribe_queue_size: 1
command_timeout: 5.0
origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id

10
config/sbpl_global_params.yaml Executable file
View File

@ -0,0 +1,10 @@
base_global_planner: SBPLLatticePlanner
SBPLLatticePlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: true
nominalvel_mpersecs: 0.2
timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s

61
config/sick_line_guidance_mls.yaml Normal file
View File

@ -0,0 +1,61 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (1/rate in seconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
f_mlse:
id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls"
subscribe_queue_size: 1
sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
#
b_mlse:
id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls"
subscribe_queue_size: 1
sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
#

3
config/two_points_global_params.yaml Normal file
View File

@ -0,0 +1,3 @@
base_global_planner: TwoPointsPlanner
TwoPointsPlanner:
lethal_obstacle: 20

446
doc/architecture_discussion.md Normal file
View File

@ -0,0 +1,446 @@
# Thảo luận về Kiến trúc Navigation cho AMR
## Mục tiêu dự án
Nghiên cứu và phát triển hệ thống navigation (di chuyển bám quỹ đạo) cho robot AMR dạng hai bánh vi sai, **không sử dụng ROS**, viết thuần C++ với CMake trên Linux.
## Yêu cầu chức năng
Từ `readme.md`:
- Có khả năng thay đổi mô hình kinematics (dạng hai bánh vi sai, dạng steering bicycle, ...)
- Có chức năng di chuyển từ điểm khởi đầu đến điểm đích
- Có chức năng xoay tròn
- Có chức năng đi thẳng
- Có chức năng thay đổi vận tốc
## Sơ đồ kiến trúc hệ thống
Kiến trúc được thiết kế theo mô hình layered, tách biệt các concerns và cho phép mở rộng dễ dàng:
```mermaid
flowchart TB
%% ========== API LAYER ==========
subgraph API["🌐 API Layer"]
direction TB
CAPI["<b>📦 C API</b><br/>━━━━━━━━━━━━━━━━<br/>🔌 nav_c_api<br/>💻 P/Invoke cho .NET/C#<br/>📝 Wrapper Functions<br/>🔗 Interop Layer"]
style API fill:#E3F2FD,stroke:#1976D2,stroke-width:4px,color:#000
style CAPI fill:#BBDEFB,stroke:#1976D2,stroke-width:3px,font-size:14px
end
%% ========== USER CONTROLLER LAYER ==========
subgraph UserController["🎮 User Controller Layer"]
direction TB
UserCtrl["<b>🔌 User Controller Plugin</b><br/>━━━━━━━━━━━━━━━━<br/>📚 Dynamic Loader<br/>⚙️ boost::dll<br/>🎯 Custom Behavior<br/>🔄 Runtime Loading"]
style UserController fill:#F3E5F5,stroke:#7B1FA2,stroke-width:4px,color:#000
style UserCtrl fill:#E1BEE7,stroke:#7B1FA2,stroke-width:3px,font-size:14px
end
%% ========== INTERFACE LAYER ==========
subgraph Interface["🔌 Interface Layer"]
direction TB
BaseNav["<b>📋 BaseNavigation</b><br/>━━━━━━━━━━━━━━━━<br/>🏗️ move_base_core::BaseNavigation<br/>🎯 Abstract Interface<br/>━━━━━━━━━━━━━━━━<br/>📍 moveTo, dockTo<br/>🔄 rotateTo, moveStraightTo<br/>⏸️ pause, resume, cancel<br/>📊 getRobotPose"]
style Interface fill:#FFF3E0,stroke:#E65100,stroke-width:4px,color:#000
style BaseNav fill:#FFE0B2,stroke:#E65100,stroke-width:3px,font-size:14px
end
%% ========== MOVE BASE CORE ==========
subgraph MoveBaseCore["⚙️ Move Base Core"]
direction TB
MoveBase["<b>🚀 MoveBase</b><br/>━━━━━━━━━━━━━━━━<br/>📦 move_base::MoveBase<br/>🔄 State Machine<br/>━━━━━━━━━━━━━━━━<br/>📊 PLANNING<br/>🎮 CONTROLLING<br/>🧹 CLEARING<br/>━━━━━━━━━━━━━━━━<br/>🎛️ Control Loop<br/>🔄 executeCycle"]
style MoveBaseCore fill:#E8F5E9,stroke:#2E7D32,stroke-width:4px,color:#000
style MoveBase fill:#C8E6C9,stroke:#2E7D32,stroke-width:3px,font-size:14px
end
%% ========== PLANNING LAYER ==========
subgraph Planning["🗺️ Planning Layer - Plugin System"]
direction LR
GP["<b>🌍 Global Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseGlobalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 A*, D*, Hybrid A*<br/>📐 Long-range Planning<br/>🗺️ Global Path"]
LP["<b>📍 Local Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseLocalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 DWA, TEB, MKT<br/>🚗 Velocity Commands<br/>🛡️ Obstacle Avoidance"]
RB["<b>🔄 Recovery Behaviors</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::RecoveryBehavior<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🧹 Clear Costmap<br/>🔄 Rotate Recovery<br/>🚨 Emergency Stop"]
style Planning fill:#E1F5FE,stroke:#0277BD,stroke-width:4px,color:#000
style GP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
style LP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
style RB fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
end
%% ========== COSTMAP LAYER ==========
subgraph Costmap["🗺️ Costmap Layer"]
direction LR
GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
end
%% ========== ALGORITHMS LAYER ==========
subgraph Algorithms["🧮 Algorithms Layer"]
direction LR
MKTAlgo["<b>🚗 MKT Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>⚙️ Diff Drive Kinematics<br/>🚲 Bicycle Kinematics<br/>📐 Trajectory Generation"]
ScoreAlgo["<b>📊 Score Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>📈 Trajectory Scoring<br/>✅ Goal Checking<br/>🎯 Path Evaluation"]
KalmanAlgo["<b>🔍 Kalman Filter</b><br/>━━━━━━━━━━━━━━━━<br/>📊 State Estimation<br/>🔮 Sensor Fusion<br/>📉 Noise Filtering"]
style Algorithms fill:#FCE4EC,stroke:#C2185B,stroke-width:4px,color:#000
style MKTAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
style ScoreAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
style KalmanAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
end
%% ========== DATA SOURCES ==========
subgraph DataSources["📡 Data Sources"]
direction TB
Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
style TF fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
style Odom fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
style Laser fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
style Map fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
end
%% ========== CONTROL LOOP ==========
subgraph ControlLoop["🔄 Control Loop"]
direction LR
CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
style BaseCtrl fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
end
%% ========== CONNECTIONS ==========
%% API to User Controller
CAPI -->|"🔗 P/Invoke"| UserCtrl
%% User Controller to Interface
UserCtrl -->|"🎯 Uses"| BaseNav
%% Interface to MoveBase
BaseNav -->|"⚙️ Implements"| MoveBase
%% MoveBase to Planning
MoveBase -->|"🎛️ Manages"| GP
MoveBase -->|"🎛️ Manages"| LP
MoveBase -->|"🎛️ Manages"| RB
%% MoveBase to Costmap
MoveBase -->|"🎛️ Manages"| GC
MoveBase -->|"🎛️ Manages"| LC
%% Data Sources to Components
Goal -->|"📥 Input"| UserCtrl
Goal -->|"📥 Input"| BaseNav
Loc -->|"📍 Pose"| MoveBase
TF -->|"🔄 Transforms"| MoveBase
Laser -->|"📡 Scan Data"| LC
Map -->|"🗺️ Static Map"| GC
Odom -->|"⚡ Velocity"| LP
Odom -->|"📍 Pose"| MoveBase
%% Planning Flow
GC -->|"🗺️ Costmap"| GP
GP -->|"🛤️ Global Path"| LP
LC -->|"🗺️ Costmap"| LP
LC -->|"🗺️ Costmap"| RB
GC -->|"🗺️ Costmap"| RB
%% Algorithm Integration
MKTAlgo -->|"🚗 Kinematics"| LP
ScoreAlgo -->|"📊 Scoring"| LP
KalmanAlgo -.->|"🔍 Filtering"| Loc
%% Control Flow
LP -->|"⚡ Velocity Cmd"| CmdVel
CmdVel -->|"▶️ Execute"| BaseCtrl
BaseCtrl -->|"📊 Feedback"| Odom
%% Styling Classes
classDef implemented fill:#C8E6C9,stroke:#2E7D32,stroke-width:4px,color:#000
classDef partial fill:#FFF9C4,stroke:#F57F17,stroke-width:3px,color:#000
classDef todo fill:#FFCDD2,stroke:#C62828,stroke-width:3px,color:#000
class BaseNav,MoveBase,GP,LP,RB,GC,LC,CAPI,MKTAlgo,ScoreAlgo,KalmanAlgo,UserCtrl implemented
class BaseCtrl,Loc,Odom,Map,Laser,TF partial
```
## Đề xuất giải pháp kiến trúc
### 1. Xác định phạm vi & yêu cầu vận hành
Cần làm rõ:
- **Kịch bản sử dụng chính**: go-to-goal trong môi trường indoor/outdoor? Tốc độ di chuyển? Độ chính xác yêu cầu?
- **Cảm biến có sẵn**: encoder, IMU, lidar, camera? Dữ liệu nào đã có?
- **Yêu cầu real-time**: chu kỳ điều khiển (control loop frequency), độ trễ tối đa cho phép?
### 2. Kiến trúc tổng thể
**Các layer chính:**
1. **API Layer** ✅
- C API (`nav_c_api`) cho P/Invoke với .NET/C#
- Wrapper functions cho tất cả BaseNavigation methods
2. **User Controller Layer** ✅
- **User Controller Plugin**: Dynamically allocated từ loader controller plugin
- Sử dụng `boost::dll` để load plugins động
- Cho phép user định nghĩa controller riêng để điều khiển navigation behavior
- Kết nối với `BaseNavigation` interface
3. **Interface Layer** ✅
- `move_base_core::BaseNavigation`: Abstract interface cho navigation
- Định nghĩa các operations: moveTo, dockTo, rotateTo, moveStraightTo, pause, resume, cancel
- Là lớp trung gian định nghĩa các phương thức và thuộc tính liên quan đến navigation
4. **Move Base Core** ✅
- `move_base::MoveBase`: Core implementation của BaseNavigation
- Quản lý state machine (PLANNING, CONTROLLING, CLEARING)
- Điều phối global/local planner và recovery behaviors
- Quản lý costmaps (global và local)
- Thực thi control loop (executeCycle)
4. **Planning Layer** ✅
- `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.)
- `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
- `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
- Plugin system sử dụng `boost::dll` để dynamic loading
5. **Costmap Layer** ✅
- `costmap_2d::Costmap2DROBOT`: Global và local costmap
- Costmap layers: static map, obstacles, inflation
- Frame management: map (global), odom (local)
6. **Algorithms Layer** ✅
- `mkt_algorithm`: Diff drive và bicycle kinematics algorithms
- `score_algorithm`: Trajectory scoring và goal checking
- `kalman`: Filtering algorithms
7. **Data Sources** ⚠️ (Interface cần định nghĩa)
- Localization source (Pnkx Loc)
- Odometry source
- Sensor transforms (tf3)
- Map server
- Laser sensors
8. **Control Layer** ⚠️ (Cần implementation)
- Base Controller interface (Diff/Steer drive)
- Velocity command execution
**Định dạng dữ liệu:**
- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
- `geometry_msgs::Twist` (vận tốc linear/angular)
- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
- `costmap_2d::Costmap2D` (bản đồ chi phí)
### 3. Thiết kế từng module (interface level)
**Các interface đã có:** ✅
- `move_base_core::BaseNavigation` ✅
- `initialize(TFListenerPtr)` - Khởi tạo với TF listener
- `moveTo(goal, xy_tol, yaw_tol)` - Di chuyển đến goal
- `dockTo(marker, goal, ...)` - Docking đến marker
- `rotateTo(goal, yaw_tol)` - Xoay tại chỗ
- `moveStraightTo(goal, xy_tol)` - Đi thẳng
- `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái
- `getRobotPose(...)` - Lấy vị trí robot
- `nav_core::BaseGlobalPlanner` ✅
- `makePlan(start, goal, plan)` - Tạo global path
- `initialize(name, costmap_robot)` - Khởi tạo với costmap
- `nav_core::BaseLocalPlanner` ✅
- `computeVelocityCommands(cmd_vel)` - Tính toán velocity command
- `setPlan(plan)` - Set global path để follow
- `isGoalReached()` - Kiểm tra đã đến goal chưa
- `swapPlanner(name)` - Thay đổi planner động
- `setTwistLinear/Angular(...)` - Set velocity limits
- `nav_core::RecoveryBehavior` ✅
- `runBehavior()` - Thực thi recovery behavior
- `costmap_2d::Costmap2DROBOT` ✅
- Wrapper cho costmap với robot footprint
- Thread-safe access với mutex
**Các interface cần bổ sung:** ⚠️
- `ILocalizationSource` ⚠️
- `getCurrentPose()` - Lấy vị trí hiện tại từ localization
- `isAvailable()` - Kiểm tra localization có sẵn không
- `IOdometrySource` ⚠️
- `getCurrentPose()` - Lấy vị trí từ odometry
- `getCurrentVelocity()` - Lấy vận tốc hiện tại
- `isAvailable()` - Kiểm tra odometry có sẵn không
- `IBaseController` ⚠️
- `executeVelocity(cmd_vel)` - Thực thi velocity command
- `stop()` - Dừng robot ngay lập tức
- `getCurrentVelocity()` - Lấy vận tốc thực tế
- `IMapProvider` ⚠️
- `getMap()` - Lấy static map
- `isMapAvailable()` - Kiểm tra map có sẵn không
**Plugin mechanism:** ✅
- Sử dụng `boost::dll` để dynamic loading plugins
- Factory pattern với `boost::function` và `boost::dll::import`
- Config file YAML để specify plugin names
- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior`
### 4. Cơ chế giao tiếp & đồng bộ
**Lựa chọn transport:**
- Shared memory + mutex (cho real-time nhẹ)
- Message queue (ZeroMQ, nanomsg)
- Event loop với callback thread-safe
**Time synchronization:**
- Sử dụng `std::chrono` cho timestamp
- Buffer dữ liệu để xử lý dữ liệu không đồng bộ
**Threading strategy:**
- Mỗi module một thread riêng, hoặc
- Scheduler chung quản lý tất cả
### 5. Chiến lược an toàn & recovery
**Monitoring:**
- Heartbeat mechanism
- Watchdog timer
**Recovery behaviors:**
- Không tìm được đường đi
- Mất localization
- Obstacle chặn đường
- Emergency stop mechanism
- Giới hạn vận tốc theo trạng thái
### 6. Config & Logging/Diagnostics
**Configuration:**
- File YAML/JSON cho:
- Kinematics parameters
- Velocity limits
- Planner parameters
- Sensor configurations
**Logging & Debugging:**
- Logging framework (spdlog?)
- Telemetry interface
- Visualizer tool (SDL/ImGui) để debug map/path
### 7. Trạng thái triển khai & lộ trình
**Đã hoàn thành:** ✅
1. ✅ **Interface Layer**: `BaseNavigation` interface
2. ✅ **Implementation Layer**: `MoveBase` core logic
3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
4. ✅ **Costmap Layer**: Global và local costmap với layers
5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
6. ✅ **API Layer**: C API wrapper cho .NET integration
7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, nav_2d_utils
**Đang triển khai / Cần bổ sung:** ⚠️
1. ⚠️ **Data Sources Interfaces**:
- `ILocalizationSource` interface
- `IOdometrySource` interface
- `IMapProvider` interface
- Integration với Pnkx Loc, odometry sources
2. ⚠️ **Base Controller**:
- `IBaseController` interface
- Diff drive controller implementation
- Steer drive controller implementation
- Velocity command execution
3. ⚠️ **Control Loop**:
- Control loop trong MoveBase (executeCycle)
- State machine management hoàn chỉnh
- Threading và synchronization
4. ⚠️ **User Controller Plugin System**:
- Factory để load user controller plugins
- Integration với BaseNavigation
**Lộ trình tiếp theo:**
**Phase 1: Data Sources & Base Controller** (Ưu tiên cao)
- Định nghĩa interfaces cho data sources
- Implement Base Controller interface và diff drive controller
- Integration với MoveBase
**Phase 2: Control Loop & State Management** (Ưu tiên cao)
- Hoàn thiện executeCycle trong MoveBase
- State machine management
- Threading strategy
**Phase 3: User Controller Plugin System** (Ưu tiên trung bình)
- Factory pattern cho user controllers
- Plugin loading mechanism
- Integration testing
**Phase 4: Testing & Optimization** (Ưu tiên trung bình)
- Unit tests cho các module
- Integration tests
- Performance optimization
**Testing strategy:**
- Unit tests cho các module độc lập (gtest?)
- Integration tests cho full navigation stack
- Simulation environment (2D simulator) - TODO
- Hardware-in-the-loop testing - TODO
## Cấu trúc thư mục
```
pnkx_nav_core/
├── src/
│ ├── Navigations/
│ │ ├── Cores/
│ │ │ ├── move_base_core/ # BaseNavigation interface
│ │ │ ├── nav_core/ # Planner interfaces
│ │ │ ├── nav_core_adapter/ # Adapter utilities
│ │ │ └── nav_core2/ # Additional nav utilities
│ │ ├── Libraries/
│ │ │ ├── costmap_2d/ # Costmap system
│ │ │ ├── tf3/ # Transform system
│ │ │ ├── robot_time/ # Time management
│ │ │ ├── geometry2/ # Geometry utilities
│ │ │ └── ... # Other supporting libraries
│ │ └── Packages/
│ │ └── move_base/ # MoveBase implementation
│ ├── Algorithms/
│ │ ├── Cores/
│ │ │ └── score_algorithm/ # Trajectory scoring
│ │ └── Libraries/
│ │ ├── mkt_algorithm/ # MKT kinematics algorithms
│ │ ├── kalman/ # Kalman filtering
│ │ └── angles/ # Angle utilities
│ └── APIs/
│ └── c_api/ # C API wrapper
├── build/ # Build artifacts
└── doc/ # Documentation
```
## Ghi chú
- ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
- ⚠️ Cần bổ sung data sources interfaces và base controller
- 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system
- 📦 Tất cả components được build bằng CMake, không phụ thuộc ROS
- 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading

1282
doc/diff_drive_c.drawio Executable file
View File

File diff suppressed because one or more lines are too long

53
doc/folders.md Normal file
View File

@ -0,0 +1,53 @@
Mô tả cấu trúc:
├── common_msgs/
│ ├── build/
│ ├── geometry_msgs/
│ │ ├── include/
│ │ └── test/
│ ├── CMakeLists.txt
│ ├── nav_msgs/
│ │ ├── include/
│ │ └── test/
│ ├── CMakeLists.txt
│ ├── sensor_msgs/
│ │ ├── cfg/
│ │ ├── include/
│ │ └── test/
│ ├── CMakeLists.txt
│ ├── std_msgs/
│ │ ├── include/
│ │ └── CMakeLists.txt
│ └── CMakeLists.txt (root)
|
├── Navigations/
│ ├── Cores/
│ │ └── move_base_core/
│ │ ├── build/
│ │ ├── example/
│ │ ├── include/
│ │ ├── .gitignore
│ │ ├── CMakeLists.txt
│ │ └── README.md
│ ├── Libraries/
│ │ ├── geometry_msgs/
│ │ │ ├── build/
│ │ │ ├── include/
│ │ │ └── CMakeLists.txt
│ │ └── libtf2/
│ │ ├── .vscode/
│ │ ├── include/
│ │ ├── samples/
│ │ ├── src/
│ │ ├── .gitignore
│ │ ├── DEBIAN_PACKAGING.md
│ │ ├── libtf2_2.0.0_amd64.deb
│ │ ├── Makefile
│ │ └── README.md
│ ├── Packages/
│ │ └── move_base/
│ │ ├── build/
│ │ ├── include/
│ │ ├── src/
│ │ └── CMakeLists.txt
│ └── CMakeLists.txt (root)

230
doc/implementation_plan.md Normal file
View File

@ -0,0 +1,230 @@
# Kế hoạch Implementation Navigation System
## Phân tích Kiến trúc Hiện tại
### ✅ Đã có sẵn:
1. **Interface Layer:**
- `move_base_core::BaseNavigation` - Interface chính
- `move_base::MoveBase` - Implementation
2. **Core Components:**
- `nav_core::BaseGlobalPlanner` - Interface cho global planner
- `nav_core::BaseLocalPlanner` - Interface cho local planner
- `nav_core::RecoveryBehavior` - Interface cho recovery behaviors
- Plugin system với `boost::dll` để load dynamic plugins
3. **Costmap System:**
- `costmap_2d::Costmap2DROBOT` - Global và local costmap
- Costmap layers (static, obstacle, inflation, etc.)
4. **Supporting Libraries:**
- `tf3` - Transform system
- `robot_time` - Time management
- `geometry_msgs` - Message types
- `nav_2d_msgs`, `nav_2d_utils` - 2D navigation utilities
### ❌ Cần bổ sung/hoàn thiện:
1. **User Controller Plugin System:**
- Factory để tạo User Controller từ plugin
- Interface cho User Controller
- Integration với BaseNavigation
2. **Data Sources Integration:**
- Pnkx Loc interface
- Odometry Source interface
- Sensor Transform management
- Map Server interface
- Laser Sensor Sources interface
3. **Base Controller:**
- Diff drive controller
- Steer drive controller
- Velocity command execution
4. **Hoàn thiện MoveBase:**
- Control loop implementation
- State machine management
- Recovery behavior execution
## Kế hoạch Implementation
### Phase 1: Data Sources & Interfaces (Ưu tiên cao)
#### 1.1 Odometry Source Interface
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
```cpp
class IOdometrySource {
virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
virtual geometry_msgs::Twist getCurrentVelocity() = 0;
virtual bool isAvailable() = 0;
};
```
#### 1.2 Localization Source Interface (Pnkx Loc)
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
```cpp
class ILocalizationSource {
virtual geometry_msgs::PoseStamped getRobotPose() = 0;
virtual std::string getGlobalFrame() = 0;
virtual bool isLocalized() = 0;
};
```
#### 1.3 Map Provider Interface
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h`
```cpp
class IMapProvider {
virtual nav_msgs::OccupancyGrid getMap() = 0;
virtual bool isMapAvailable() = 0;
};
```
#### 1.4 Sensor Data Interface
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h`
```cpp
class ISensorSource {
virtual sensor_msgs::LaserScan getLatestScan() = 0;
virtual bool hasNewData() = 0;
};
```
### Phase 2: Base Controller (Ưu tiên cao)
#### 2.1 Base Controller Interface
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
```cpp
class IBaseController {
virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
virtual bool stop() = 0;
virtual geometry_msgs::Twist getCurrentVelocity() = 0;
};
```
#### 2.2 Diff Drive Controller
**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/diff_drive_controller.h`
- Implement IBaseController
- Convert Twist to wheel velocities
- Handle kinematics cho differential drive
#### 2.3 Steer Drive Controller
**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/steer_drive_controller.h`
- Implement IBaseController
- Convert Twist to steering angles và wheel velocities
- Handle kinematics cho bicycle/steering model
### Phase 3: User Controller Plugin System (Ưu tiên trung bình)
#### 3.1 User Controller Interface
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
```cpp
class IUserController {
virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
virtual void onNavigationStateChanged(move_base_core::State state) = 0;
virtual void onFeedback(const NavFeedback& feedback) = 0;
};
```
#### 3.2 User Controller Factory
**File:** `src/Navigations/Cores/move_base_core/src/user_controller_factory.cpp`
- Load plugin từ config
- Create instance với boost::dll
- Manage lifecycle
### Phase 4: Control Loop & State Machine (Ưu tiên cao)
#### 4.1 Control Loop Implementation
**File:** `src/Navigations/Packages/move_base/src/move_base_control_loop.cpp`
- Main control loop thread
- Planner thread management
- Controller execution
- Frequency control
#### 4.2 State Machine
**File:** `src/Navigations/Packages/move_base/src/move_base_state_machine.cpp`
- State transitions
- Recovery behavior triggers
- Goal management
### Phase 5: Integration & Testing (Ưu tiên trung bình)
#### 5.1 Integration Layer
- Connect tất cả components
- Data flow management
- Error handling
#### 5.2 Testing
- Unit tests
- Integration tests
- Simulation tests
## Cấu trúc Thư mục Đề xuất
```
src/Navigations/
├── Cores/
│ ├── move_base_core/ ✅ Đã có
│ │ ├── interfaces/
│ │ │ ├── odometry_source.h ⚠️ Cần tạo
│ │ │ ├── localization_source.h ⚠️ Cần tạo
│ │ │ ├── map_provider.h ⚠️ Cần tạo
│ │ │ ├── sensor_source.h ⚠️ Cần tạo
│ │ │ ├── base_controller.h ⚠️ Cần tạo
│ │ │ └── user_controller.h ⚠️ Cần tạo
│ │ └── src/
│ │ └── user_controller_factory.cpp ⚠️ Cần tạo
│ ├── nav_core/ ✅ Đã có
│ └── nav_core2/ ✅ Đã có
│
├── Packages/
│ ├── move_base/ ✅ Đã có (cần hoàn thiện)
│ │ └── src/
│ │ ├── move_base_control_loop.cpp ⚠️ Cần tạo
│ │ └── move_base_state_machine.cpp ⚠️ Cần tạo
│ │
│ └── base_controllers/ ⚠️ Cần tạo
│ ├── include/
│ │ └── base_controllers/
│ │ ├── diff_drive_controller.h
│ │ └── steer_drive_controller.h
│ └── src/
│ ├── diff_drive_controller.cpp
│ └── steer_drive_controller.cpp
│
└── Libraries/
└── (các libraries hiện có) ✅
```
## Thứ tự Implementation
### Sprint 1: Foundation (1-2 tuần)
1. ✅ Tạo interfaces cho data sources
2. ✅ Tạo base controller interface
3. ✅ Implement diff drive controller cơ bản
### Sprint 2: Integration (1-2 tuần)
1. ✅ Integrate data sources vào MoveBase
2. ✅ Implement control loop
3. ✅ Connect base controller
### Sprint 3: Plugin System (1 tuần)
1. ✅ User controller plugin system
2. ✅ Factory pattern implementation
### Sprint 4: State Machine & Recovery (1 tuần)
1. ✅ State machine implementation
2. ✅ Recovery behavior integration
### Sprint 5: Testing & Polish (1 tuần)
1. ✅ Unit tests
2. ✅ Integration tests
3. ✅ Documentation
## Notes
- Tất cả interfaces nên follow pattern của `BaseNavigation`
- Sử dụng `boost::dll` cho plugin system (đã có sẵn)
- Thread safety: sử dụng `boost::mutex` và `boost::recursive_mutex`
- Error handling: throw exceptions, return bool cho simple operations

15
doc/readme.md Normal file
View File

@ -0,0 +1,15 @@
Mô tả dựng án :
Tôi đang nghiên cứu viết chương di chuyển bám quỹ đạo (navigation) cho robot amr dạng hai bánh vi sai. Tôi muốn bỏ ROS chuyển sang viết
thuần trên Cmake.
Các chức năng cần có:
- Có khả năng thay đổi mô hình kinematics như dạng hai bánh vi sai, dạng steering bicycle, ...
- Có chức năng di chuyển từ điểủ khởi đầu đến điểm đích
- Có chức năng xoay tròn
- Có chức năng đi thẳng
- Có chức năng thay đổi vận tốc

322
examples/CSharpExample.cs Normal file
View File

@ -0,0 +1,322 @@
using System;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;
namespace NavigationExample
{
/// <summary>
/// C# P/Invoke wrapper for Navigation C API
/// </summary>
public class NavigationAPI
{
private const string DllName = "libnav_c_api.so"; // Linux
// For Windows: "nav_c_api.dll"
// For macOS: "libnav_c_api.dylib"
// ============================================================================
// Enums
// ============================================================================
public enum NavigationState
{
Pending = 0,
Active = 1,
Preempted = 2,
Succeeded = 3,
Aborted = 4,
Rejected = 5,
Preempting = 6,
Recalling = 7,
Recalled = 8,
Lost = 9,
Planning = 10,
Controlling = 11,
Clearing = 12,
Paused = 13
}
// ============================================================================
// Structures
// ============================================================================
[StructLayout(LayoutKind.Sequential)]
public struct Point
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct Pose2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
public double x;
public double y;
public double z;
public double w;
}
[StructLayout(LayoutKind.Sequential)]
public struct Position
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct Pose
{
public Position position;
public Quaternion orientation;
}
[StructLayout(LayoutKind.Sequential)]
public struct Header
{
public uint seq;
public long sec;
public uint nsec;
public IntPtr frame_id; // char*
}
[StructLayout(LayoutKind.Sequential)]
public struct PoseStamped
{
public Header header;
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct NavFeedback
{
public NavigationState navigation_state;
public IntPtr feed_back_str; // char*
public Pose2D current_pose;
[MarshalAs(UnmanagedType.I1)]
public bool goal_checked;
[MarshalAs(UnmanagedType.I1)]
public bool is_ready;
}
// ============================================================================
// String Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void nav_c_api_free_string(IntPtr str);
// ============================================================================
// State Conversion
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
public static extern IntPtr navigation_state_to_string(NavigationState state);
// ============================================================================
// Helper Functions
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_offset_goal_2d(
double pose_x, double pose_y, double pose_theta,
string frame_id, double offset_distance,
ref PoseStamped out_goal);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_offset_goal_stamped(
ref PoseStamped in_pose, double offset_distance,
ref PoseStamped out_goal);
// ============================================================================
// Navigation Handle Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr navigation_create();
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_destroy(IntPtr handle);
// ============================================================================
// TF Listener Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr tf_listener_create();
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void tf_listener_destroy(IntPtr handle);
// ============================================================================
// Navigation Interface Methods
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
IntPtr handle, ref PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_dock_to(
IntPtr handle, string marker, ref PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_straight_to(
IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_rotate_to(
IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_pause(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_resume(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_cancel(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_twist_linear(
IntPtr handle, double linear_x, double linear_y, double linear_z);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_twist_angular(
IntPtr handle, double angular_x, double angular_y, double angular_z);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_pose_stamped(
IntPtr handle, ref PoseStamped out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(
IntPtr handle, ref NavFeedback out_feedback);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_feedback(ref NavFeedback feedback);
// ============================================================================
// Helper Methods for String Conversion
// ============================================================================
public static string MarshalString(IntPtr ptr)
{
if (ptr == IntPtr.Zero)
return string.Empty;
return Marshal.PtrToStringAnsi(ptr);
}
}
// ============================================================================
// Example Usage
// ============================================================================
class Program
{
// Helper method để hiển thị file và line number tự động
static void LogError(string message,
[CallerFilePath] string filePath = "",
[CallerLineNumber] int lineNumber = 0,
[CallerMemberName] string memberName = "")
{
// Lấy tên file từ đường dẫn đầy đủ
string fileName = System.IO.Path.GetFileName(filePath);
Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
}
static void Main(string[] args)
{
// Create TF listener
IntPtr tfHandle = NavigationAPI.tf_listener_create();
if (tfHandle == IntPtr.Zero)
{
LogError("Failed to create TF listener");
return;
}
// Create navigation instance
IntPtr navHandle = NavigationAPI.navigation_create();
if (navHandle == IntPtr.Zero)
{
LogError("Failed to create navigation instance");
NavigationAPI.tf_listener_destroy(tfHandle);
return;
}
// Initialize navigation
if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
{
LogError("Failed to initialize navigation");
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
return;
}
// Set robot footprint
NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
{
new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
};
NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
// Get robot pose
NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
{
Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
}
// Get navigation feedback
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
{
string stateStr = NavigationAPI.MarshalString(
NavigationAPI.navigation_state_to_string(feedback.navigation_state));
string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
NavigationAPI.navigation_free_feedback(ref feedback);
}
// Cleanup
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
}
}
}

13
examples/NavigationExample/NavigationExample.csproj Normal file
View File

@ -0,0 +1,13 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net6.0</TargetFramework>
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<None Include="libnav_c_api.so">
<CopyToOutputDirectory>Always</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>

322
examples/NavigationExample/Program.cs Normal file
View File

@ -0,0 +1,322 @@
using System;
using System.Runtime.InteropServices;
using System.Runtime.CompilerServices;
namespace NavigationExample
{
/// <summary>
/// C# P/Invoke wrapper for Navigation C API
/// </summary>
public class NavigationAPI
{
private const string DllName = "libnav_c_api.so"; // Linux
// For Windows: "nav_c_api.dll"
// For macOS: "libnav_c_api.dylib"
// ============================================================================
// Enums
// ============================================================================
public enum NavigationState
{
Pending = 0,
Active = 1,
Preempted = 2,
Succeeded = 3,
Aborted = 4,
Rejected = 5,
Preempting = 6,
Recalling = 7,
Recalled = 8,
Lost = 9,
Planning = 10,
Controlling = 11,
Clearing = 12,
Paused = 13
}
// ============================================================================
// Structures
// ============================================================================
[StructLayout(LayoutKind.Sequential)]
public struct Point
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct Pose2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
public double x;
public double y;
public double z;
public double w;
}
[StructLayout(LayoutKind.Sequential)]
public struct Position
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct Pose
{
public Position position;
public Quaternion orientation;
}
[StructLayout(LayoutKind.Sequential)]
public struct Header
{
public uint seq;
public long sec;
public uint nsec;
public IntPtr frame_id; // char*
}
[StructLayout(LayoutKind.Sequential)]
public struct PoseStamped
{
public Header header;
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
public double x;
public double y;
public double z;
}
[StructLayout(LayoutKind.Sequential)]
public struct NavFeedback
{
public NavigationState navigation_state;
public IntPtr feed_back_str; // char*
public Pose2D current_pose;
[MarshalAs(UnmanagedType.I1)]
public bool goal_checked;
[MarshalAs(UnmanagedType.I1)]
public bool is_ready;
}
// ============================================================================
// String Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void nav_c_api_free_string(IntPtr str);
// ============================================================================
// State Conversion
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
public static extern IntPtr navigation_state_to_string(NavigationState state);
// ============================================================================
// Helper Functions
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_offset_goal_2d(
double pose_x, double pose_y, double pose_theta,
string frame_id, double offset_distance,
ref PoseStamped out_goal);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_offset_goal_stamped(
ref PoseStamped in_pose, double offset_distance,
ref PoseStamped out_goal);
// ============================================================================
// Navigation Handle Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr navigation_create();
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_destroy(IntPtr handle);
// ============================================================================
// TF Listener Management
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern IntPtr tf_listener_create();
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void tf_listener_destroy(IntPtr handle);
// ============================================================================
// Navigation Interface Methods
// ============================================================================
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
IntPtr handle, ref PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_dock_to(
IntPtr handle, string marker, ref PoseStamped goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_straight_to(
IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_rotate_to(
IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_pause(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_resume(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_cancel(IntPtr handle);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_twist_linear(
IntPtr handle, double linear_x, double linear_y, double linear_z);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_set_twist_angular(
IntPtr handle, double angular_x, double angular_y, double angular_z);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_pose_stamped(
IntPtr handle, ref PoseStamped out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(
IntPtr handle, ref NavFeedback out_feedback);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_feedback(ref NavFeedback feedback);
// ============================================================================
// Helper Methods for String Conversion
// ============================================================================
public static string MarshalString(IntPtr ptr)
{
if (ptr == IntPtr.Zero)
return string.Empty;
return Marshal.PtrToStringAnsi(ptr);
}
}
// ============================================================================
// Example Usage
// ============================================================================
class Program
{
// Helper method để hiển thị file và line number tự động
static void LogError(string message,
[CallerFilePath] string filePath = "",
[CallerLineNumber] int lineNumber = 0,
[CallerMemberName] string memberName = "")
{
// Lấy tên file từ đường dẫn đầy đủ
string fileName = System.IO.Path.GetFileName(filePath);
Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
}
static void Main(string[] args)
{
// Create TF listener
IntPtr tfHandle = NavigationAPI.tf_listener_create();
if (tfHandle == IntPtr.Zero)
{
LogError("Failed to create TF listener");
return;
}
// Create navigation instance
IntPtr navHandle = NavigationAPI.navigation_create();
if (navHandle == IntPtr.Zero)
{
LogError("Failed to create navigation instance");
NavigationAPI.tf_listener_destroy(tfHandle);
return;
}
// Initialize navigation
if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
{
LogError("Failed to initialize navigation");
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
return;
}
// Set robot footprint
NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
{
new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
};
NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
// Get robot pose
NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
{
Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
}
// Get navigation feedback
NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
{
string stateStr = NavigationAPI.MarshalString(
NavigationAPI.navigation_state_to_string(feedback.navigation_state));
string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
NavigationAPI.navigation_free_feedback(ref feedback);
}
// Cleanup
NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle);
}
}
}

BIN
examples/NavigationExample/libnav_c_api.so Executable file
View File

Binary file not shown.

148
examples/QUICK_START.md Normal file
View File

@ -0,0 +1,148 @@
# Quick Start - Chạy C# Example
## Bước 1: Cài đặt .NET SDK (nếu chưa có)
### Ubuntu/Debian:
```bash
# Cài đặt .NET 6.0 hoặc mới hơn
wget https://dot.net/v1/dotnet-install.sh
chmod +x dotnet-install.sh
./dotnet-install.sh --channel 6.0
# Thêm vào PATH
export PATH=$PATH:$HOME/.dotnet
export DOTNET_ROOT=$HOME/.dotnet
# Hoặc cài qua apt (nếu có)
sudo apt-get update
sudo apt-get install -y dotnet-sdk-6.0
```
### Kiểm tra:
```bash
dotnet --version
```
## Bước 2: Build C API Library
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
mkdir -p build
cd build
cmake ..
make nav_c_api
```
Library sẽ ở: `build/src/APIs/c_api/libnav_c_api.so`
## Bước 3: Chạy C# Example
### Cách 1: Dùng script tự động (Khuyến nghị)
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
chmod +x run_example.sh
./run_example.sh
```
Script sẽ tự động:
- ✅ Build library nếu chưa có
- ✅ Tạo C# project
- ✅ Copy library và source code
- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies
- ✅ Build và chạy example
### Cách 2: Chạy thủ công
```bash
# 1. Tạo project
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
dotnet new console -n NavigationExample
cd NavigationExample
# 2. Copy source code
cp ../CSharpExample.cs Program.cs
# 3. Tạo .csproj
cat > NavigationExample.csproj << 'EOF'
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net6.0</TargetFramework>
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<None Include="libnav_c_api.so">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>
EOF
# 4. Copy library
cp ../../build/src/APIs/c_api/libnav_c_api.so .
# 5. Set library path (quan trọng!)
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
# 6. Build và chạy
dotnet build
dotnet run
```
## Bước 4: Kiểm tra kết quả
Nếu chạy thành công, bạn sẽ thấy output như:
```
Robot pose: x=0.0, y=0.0, theta=0.0
State: PENDING, Feedback: ...
```
## Troubleshooting
### Lỗi: "dotnet: command not found"
→ Cài đặt .NET SDK (xem Bước 1)
### Lỗi: "Unable to load DLL 'libnav_c_api.so'"
```bash
# Kiểm tra library có tồn tại không
ls -la ../../build/src/APIs/c_api/libnav_c_api.so
# Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
# Hoặc copy vào system lib
sudo cp libnav_c_api.so /usr/local/lib/
sudo ldconfig
```
### Lỗi: "Missing dependencies"
```bash
# Kiểm tra dependencies
ldd libnav_c_api.so
# Đảm bảo tất cả dependencies đều có sẵn
```
### Lỗi: "Failed to create navigation instance"
→ Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
Nếu vẫn gặp lỗi, kiểm tra:
- Library đã được build đầy đủ: `make nav_c_api` trong build directory
- Dependencies đã được build: `make move_base costmap_2d`
- LD_LIBRARY_PATH đã được set đúng

142
examples/README.md Normal file
View File

@ -0,0 +1,142 @@
# Hướng dẫn chạy C# Example
## Yêu cầu
1. **.NET SDK** (version 6.0 trở lên hoặc .NET 10)
2. **C API Library** đã được build (`libnav_c_api.so`)
3. **Linux environment** (hoặc WSL nếu dùng Windows)
## Bước 1: Build C API Library
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
mkdir -p build
cd build
cmake ..
make nav_c_api
```
Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so`
## Bước 2: Chạy script tự động (Khuyến nghị)
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
chmod +x run_example.sh
./run_example.sh
```
Script sẽ tự động:
- Build library nếu chưa có
- Tạo C# project
- Copy library và source code
- Set LD_LIBRARY_PATH cho tất cả dependencies
- Build và chạy example
## Bước 3: Hoặc chạy thủ công
### Tạo C# Project
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
dotnet new console -n NavigationExample
cd NavigationExample
```
### Copy file và library
```bash
# Copy C# source code
cp ../CSharpExample.cs Program.cs
# Copy library vào thư mục project
cp ../../build/src/APIs/c_api/libnav_c_api.so .
```
## Bước 4: Cập nhật .csproj (nếu cần)
Tạo file `NavigationExample.csproj`:
```xml
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net6.0</TargetFramework>
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<None Include="libnav_c_api.so">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>
```
## Bước 4: Build và chạy
```bash
# Set LD_LIBRARY_PATH để tìm được tất cả dependencies
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH
# Build
dotnet build
# Chạy
dotnet run
```
## Lưu ý
1. **Library path**: Đảm bảo `libnav_c_api.so` có thể được tìm thấy:
- Copy vào cùng thư mục với executable
- Hoặc set `LD_LIBRARY_PATH` environment variable
- Hoặc copy vào `/usr/local/lib` và chạy `ldconfig`
2. **Dependencies**: Library cần các dependencies khác:
```bash
# Kiểm tra dependencies
ldd libnav_c_api.so
# Đảm bảo tất cả dependencies đều có sẵn
```
3. **Permissions**: Đảm bảo file có quyền execute:
```bash
chmod +x libnav_c_api.so
```
## Troubleshooting
### Lỗi: "Unable to load DLL 'libnav_c_api.so'"
**Giải pháp:**
```bash
# Set LD_LIBRARY_PATH
export LD_LIBRARY_PATH=/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/APIs/c_api:$LD_LIBRARY_PATH
# Hoặc copy library vào system path
sudo cp libnav_c_api.so /usr/local/lib/
sudo ldconfig
```
### Lỗi: "DllNotFoundException"
**Giải pháp:**
- Kiểm tra tên file library có đúng không
- Kiểm tra architecture (x64 vs x86)
- Kiểm tra dependencies: `ldd libnav_c_api.so`
### Lỗi: "Symbol not found"
**Giải pháp:**
- Đảm bảo library đã được build đầy đủ
- Kiểm tra tên hàm trong C# có khớp với C API không

83
examples/START_HERE.md Normal file
View File

@ -0,0 +1,83 @@
# 🚀 Hướng dẫn chạy thử C API Navigation
## Cách nhanh nhất (1 lệnh)
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
chmod +x run_example.sh
./run_example.sh
```
## Chi tiết các bước
### Bước 1: Build C API Library
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build
cmake ..
make nav_c_api
```
Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so`
### Bước 2: Chạy C# Example
Script `run_example.sh` sẽ tự động:
- ✅ Kiểm tra và build library nếu cần
- ✅ Tạo C# project
- ✅ Copy library và source code
- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies
- ✅ Build và chạy example
```bash
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples
./run_example.sh
```
## Kết quả mong đợi
Nếu chạy thành công, bạn sẽ thấy:
```
==========================================
Building and Running C# Navigation Example
==========================================
Library already exists: .../build/src/APIs/c_api/libnav_c_api.so
C# project already exists
Copying library...
Building C# project...
Running example...
==========================================
Robot pose: x=0, y=0, theta=0
State: PENDING, Feedback:
==========================================
Done!
```
## Yêu cầu
1. **.NET SDK 6.0+** (script sẽ kiểm tra và hướng dẫn nếu thiếu)
2. **Library đã được build** (script sẽ tự động build nếu thiếu)
3. **Linux environment**
## Troubleshooting
### Lỗi: "dotnet: command not found"
→ Script sẽ hiển thị hướng dẫn cài đặt .NET SDK
### Lỗi: "Failed to create navigation instance"
→ Đảm bảo `move_base` đã được build:
```bash
cd build && make move_base
```
### Lỗi: "Cannot load library"
→ Kiểm tra LD_LIBRARY_PATH đã được set đúng (script tự động set)
## Xem thêm
- `README.md` - Hướng dẫn chi tiết
- `QUICK_START.md` - Quick start guide
- `run_example.sh` - Script tự động

1888
examples/dotnet-install.sh vendored Executable file
View File

File diff suppressed because it is too large Load Diff

117
examples/run_example.sh Executable file
View File

@ -0,0 +1,117 @@
#!/bin/bash
# Script để build và chạy C# example
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
BUILD_DIR="$PROJECT_ROOT/build"
LIB_DIR="$BUILD_DIR/src/APIs/c_api"
EXAMPLE_DIR="$SCRIPT_DIR"
echo "=========================================="
echo "Building and Running C# Navigation Example"
echo "=========================================="
echo ""
echo "📁 Paths:"
echo " SCRIPT_DIR: $SCRIPT_DIR"
echo " PROJECT_ROOT: $PROJECT_ROOT"
echo " BUILD_DIR: $BUILD_DIR"
echo " LIB_DIR: $LIB_DIR"
echo " EXAMPLE_DIR: $EXAMPLE_DIR"
echo ""
echo "Building C API library..."
cd "$BUILD_DIR"
cmake ..
make
echo "Library built successfully!"
# Kiểm tra dotnet có được cài đặt không
if ! command -v dotnet &> /dev/null; then
echo "=========================================="
echo "WARNING: .NET SDK not found!"
echo "=========================================="
echo "To run the C# example, please install .NET SDK:"
echo ""
echo " Ubuntu/Debian:"
echo " wget https://dot.net/v1/dotnet-install.sh -O dotnet-install.sh"
echo " chmod +x dotnet-install.sh"
echo " ./dotnet-install.sh --channel 6.0"
echo " export PATH=\$PATH:\$HOME/.dotnet"
echo ""
echo " Or visit: https://dotnet.microsoft.com/download"
echo ""
echo "Skipping C# example. Library is available at:"
echo " $LIB_DIR/libnav_c_api.so"
echo "=========================================="
exit 0
fi
# Bước 2: Tạo C# project nếu chưa có
if [ ! -d "$EXAMPLE_DIR/NavigationExample" ]; then
echo "Creating C# project..."
cd "$EXAMPLE_DIR"
dotnet new console -n NavigationExample
cd NavigationExample
# Tạo .csproj file
cat > NavigationExample.csproj << 'EOF'
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>net6.0</TargetFramework>
<RuntimeIdentifier>linux-x64</RuntimeIdentifier>
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
</PropertyGroup>
<ItemGroup>
<None Include="libnav_c_api.so">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
</None>
</ItemGroup>
</Project>
EOF
echo "C# project created!"
else
echo "C# project already exists"
fi
# Luôn copy source code mới nhất (cập nhật file nếu đã có)
cd "$EXAMPLE_DIR/NavigationExample"
echo "Updating Program.cs from CSharpExample.cs..."
cp ../CSharpExample.cs Program.cs
# Bước 3: Copy library
echo "Copying library..."
cp "$LIB_DIR/libnav_c_api.so" .
# Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH"
# Bước 5: Build và chạy
echo "Building C# project..."
dotnet build
echo ""
echo "Running example..."
echo "=========================================="
echo "LD_LIBRARY_PATH includes:"
echo "$LD_LIBRARY_PATH" | tr ':' '\n' | head -5
echo "..."
echo "=========================================="
dotnet run
echo "=========================================="
echo "Done!"

116
setup_catkin_workspace.sh Executable file
View File

@ -0,0 +1,116 @@
#!/bin/bash
# ========================================================
# Script to setup Catkin workspace for pnkx_nav_core
# ========================================================
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WORKSPACE_NAME="pnkx_nav_catkin_ws"
WORKSPACE_DIR="${SCRIPT_DIR}/../${WORKSPACE_NAME}"
echo "=========================================="
echo "Setting up Catkin workspace for pnkx_nav_core"
echo "=========================================="
echo ""
# Create workspace directory
echo "Creating workspace directory: ${WORKSPACE_DIR}"
mkdir -p "${WORKSPACE_DIR}/src"
# Check if src directory already has packages
if [ -d "${WORKSPACE_DIR}/src" ] && [ "$(ls -A ${WORKSPACE_DIR}/src 2>/dev/null)" ]; then
echo "Warning: ${WORKSPACE_DIR}/src already contains packages"
read -p "Do you want to continue? (y/n) " -n 1 -r
echo
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
echo "Aborted."
exit 1
fi
fi
# Create symlinks to packages that have package.xml
echo ""
echo "Creating symlinks to packages with package.xml..."
# Find all packages with package.xml in the source tree
PACKAGES=(
"src/Libraries/tf3"
"src/Libraries/nav_2d_utils"
"src/Navigations/Libraries/nav_grid"
"src/Navigations/Cores/nav_core"
"src/Navigations/Cores/nav_core2"
"src/Navigations/Cores/nav_core_adapter"
"src/Navigations/Cores/move_base_core"
"src/Navigations/Cores/nav_core_adapter"
"src/Packages/move_base"
"src/Navigations/Packages/move_base"
)
for PKG_PATH in "${PACKAGES[@]}"; do
FULL_PATH="${SCRIPT_DIR}/${PKG_PATH}"
if [ -f "${FULL_PATH}/package.xml" ]; then
PKG_NAME=$(basename "${PKG_PATH}")
TARGET="${WORKSPACE_DIR}/src/${PKG_NAME}"
if [ -e "${TARGET}" ]; then
echo " Warning: ${TARGET} already exists, skipping..."
else
echo " Linking: ${PKG_NAME} -> ${PKG_PATH}"
ln -s "${FULL_PATH}" "${TARGET}"
fi
else
echo " Warning: ${PKG_PATH}/package.xml not found, skipping..."
fi
done
# Create a README in the workspace
cat > "${WORKSPACE_DIR}/README.md" << EOF
# Catkin Workspace for pnkx_nav_core
This is a Catkin workspace containing ROS-compatible packages from pnkx_nav_core.
## Structure
\`\`\`
${WORKSPACE_NAME}/
├── src/ # Source packages (symlinked from pnkx_nav_core)
└── build/ # Build directory (created by catkin_make)
\`\`\`
## Building
\`\`\`bash
cd ${WORKSPACE_DIR}
catkin_make
\`\`\`
## Source the workspace
After building, source the setup file:
\`\`\`bash
source devel/setup.bash
\`\`\`
## Note
Packages in \`src/\` are symlinked from the main pnkx_nav_core project.
To add more packages, create symlinks in \`src/\` pointing to packages
that have both \`CMakeLists.txt\` and \`package.xml\`.
EOF
echo ""
echo "=========================================="
echo "Workspace setup complete!"
echo "=========================================="
echo ""
echo "Next steps:"
echo " 1. cd ${WORKSPACE_DIR}"
echo " 2. catkin_make"
echo " 3. source devel/setup.bash"
echo ""
echo "Note: Make sure all dependencies are built in the main project first"
echo " using standalone CMake before building with catkin."
echo ""

82
src/APIs/c_api/CMakeLists.txt Normal file
View File

@ -0,0 +1,82 @@
cmake_minimum_required(VERSION 3.10)
project(nav_c_api VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# Dependencies
set(PACKAGES_DIR
move_base_core
move_base
tf3
robot_time
geometry_msgs
)
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tìm tất cả file source
file(GLOB SOURCES "src/*.cpp")
file(GLOB HEADERS "include/*.h")
# Tạo thư viện shared (.so)
add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
# Link libraries
target_link_libraries(nav_c_api
PUBLIC
${PACKAGES_DIR}
)
# Set include directories
target_include_directories(nav_c_api
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# Cài đặt library
install(TARGETS nav_c_api
EXPORT nav_c_api-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)
# Export targets
install(EXPORT nav_c_api-targets
FILE nav_c_api-targets.cmake
DESTINATION lib/cmake/nav_c_api
)
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "=================================")

349
src/APIs/c_api/include/nav_c_api.h Normal file
View File

@ -0,0 +1,349 @@
#ifndef NAVIGATION_C_API_H
#define NAVIGATION_C_API_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
// Forward declarations
typedef void* NavigationHandle;
typedef void* TFListenerHandle;
// ============================================================================
// Enums
// ============================================================================
/**
* @brief Navigation states, including planning and controller status
*/
typedef enum {
NAV_STATE_PENDING = 0,
NAV_STATE_ACTIVE = 1,
NAV_STATE_PREEMPTED = 2,
NAV_STATE_SUCCEEDED = 3,
NAV_STATE_ABORTED = 4,
NAV_STATE_REJECTED = 5,
NAV_STATE_PREEMPTING = 6,
NAV_STATE_RECALLING = 7,
NAV_STATE_RECALLED = 8,
NAV_STATE_LOST = 9,
NAV_STATE_PLANNING = 10,
NAV_STATE_CONTROLLING = 11,
NAV_STATE_CLEARING = 12,
NAV_STATE_PAUSED = 13
} NavigationState;
// ============================================================================
// Structures
// ============================================================================
/**
* @brief Point structure (x, y, z)
*/
typedef struct {
double x;
double y;
double z;
} Point;
/**
* @brief Pose2D structure (x, y, theta)
*/
typedef struct {
double x;
double y;
double theta;
} Pose2D;
/**
* @brief Quaternion structure
*/
typedef struct {
double x;
double y;
double z;
double w;
} Quaternion;
/**
* @brief Position structure
*/
typedef struct {
double x;
double y;
double z;
} Position;
/**
* @brief Pose structure
*/
typedef struct {
Position position;
Quaternion orientation;
} Pose;
/**
* @brief Header structure
*/
typedef struct {
uint32_t seq;
int64_t sec;
uint32_t nsec;
char* frame_id;
} Header;
/**
* @brief PoseStamped structure
*/
typedef struct {
Header header;
Pose pose;
} PoseStamped;
/**
* @brief Vector3 structure
*/
typedef struct {
double x;
double y;
double z;
} Vector3;
/**
* @brief Navigation feedback structure
*/
typedef struct {
NavigationState navigation_state;
char* feed_back_str;
Pose2D current_pose;
bool goal_checked;
bool is_ready;
} NavFeedback;
// ============================================================================
// String Management
// ============================================================================
/**
* @brief Free a string allocated by the library
* @param str String to free
*/
void nav_c_api_free_string(char* str);
// ============================================================================
// State Conversion
// ============================================================================
/**
* @brief Convert a State enum to its string representation
* @param state Enum value of NavigationState
* @return String representation (caller must free with nav_c_api_free_string)
*/
char* navigation_state_to_string(NavigationState state);
// ============================================================================
// Helper Functions
// ============================================================================
/**
* @brief Creates a target pose by offsetting a given 2D pose along its heading direction
* @param pose_x X coordinate of the original pose
* @param pose_y Y coordinate of the original pose
* @param pose_theta Heading angle in radians
* @param frame_id The coordinate frame ID (null-terminated string)
* @param offset_distance Distance to offset along heading (positive = forward, negative = backward)
* @param out_goal Output parameter for the offset pose
* @return true on success, false on failure
*/
bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
const char* frame_id, double offset_distance,
PoseStamped* out_goal);
/**
* @brief Creates an offset target pose from a given PoseStamped
* @param in_pose Input pose
* @param offset_distance Distance to offset along heading direction
* @param out_goal Output parameter for the offset pose
* @return true on success, false on failure
*/
bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
PoseStamped* out_goal);
// ============================================================================
// Navigation Handle Management
// ============================================================================
/**
* @brief Create a new navigation instance
* @return Navigation handle, or NULL on failure
*/
NavigationHandle navigation_create(void);
/**
* @brief Destroy a navigation instance
* @param handle Navigation handle to destroy
*/
void navigation_destroy(NavigationHandle handle);
// ============================================================================
// TF Listener Management
// ============================================================================
/**
* @brief Create a TF listener instance
* @return TF listener handle, or NULL on failure
*/
TFListenerHandle tf_listener_create(void);
/**
* @brief Destroy a TF listener instance
* @param handle TF listener handle to destroy
*/
void tf_listener_destroy(TFListenerHandle handle);
// ============================================================================
// Navigation Interface Methods
// ============================================================================
/**
* @brief Initialize the navigation system
* @param handle Navigation handle
* @param tf_handle TF listener handle
* @return true on success, false on failure
*/
bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
/**
* @brief Set the robot's footprint (outline shape)
* @param handle Navigation handle
* @param points Array of points representing the footprint polygon
* @param point_count Number of points in the array
* @return true on success, false on failure
*/
bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
/**
* @brief Send a goal for the robot to navigate to
* @param handle Navigation handle
* @param goal Target pose in the global frame
* @param xy_goal_tolerance Acceptable error in X/Y (meters)
* @param yaw_goal_tolerance Acceptable angular error (radians)
* @return true if goal was accepted and sent successfully
*/
bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Send a docking goal to a predefined marker
* @param handle Navigation handle
* @param marker Marker name or ID (null-terminated string)
* @param goal Target pose for docking
* @param xy_goal_tolerance Acceptable XY error (meters)
* @param yaw_goal_tolerance Acceptable heading error (radians)
* @return true if docking command succeeded
*/
bool navigation_dock_to(NavigationHandle handle, const char* marker,
const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance);
/**
* @brief Move straight toward the target position
* @param handle Navigation handle
* @param goal Target pose
* @param xy_goal_tolerance Acceptable positional error (meters)
* @return true if command issued successfully
*/
bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance);
/**
* @brief Rotate in place to align with target orientation
* @param handle Navigation handle
* @param goal Pose containing desired heading (only Z-axis used)
* @param yaw_goal_tolerance Acceptable angular error (radians)
* @return true if rotation command was sent successfully
*/
bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
double yaw_goal_tolerance);
/**
* @brief Pause the robot's movement
* @param handle Navigation handle
*/
void navigation_pause(NavigationHandle handle);
/**
* @brief Resume motion after a pause
* @param handle Navigation handle
*/
void navigation_resume(NavigationHandle handle);
/**
* @brief Cancel the current goal and stop the robot
* @param handle Navigation handle
*/
void navigation_cancel(NavigationHandle handle);
/**
* @brief Send limited linear velocity command
* @param handle Navigation handle
* @param linear_x Linear velocity in X direction
* @param linear_y Linear velocity in Y direction
* @param linear_z Linear velocity in Z direction
* @return true if the command was accepted
*/
bool navigation_set_twist_linear(NavigationHandle handle,
double linear_x, double linear_y, double linear_z);
/**
* @brief Send limited angular velocity command
* @param handle Navigation handle
* @param angular_x Angular velocity around X axis
* @param angular_y Angular velocity around Y axis
* @param angular_z Angular velocity around Z axis
* @return true if the command was accepted
*/
bool navigation_set_twist_angular(NavigationHandle handle,
double angular_x, double angular_y, double angular_z);
/**
* @brief Get the robot's pose as a PoseStamped
* @param handle Navigation handle
* @param out_pose Output parameter with the robot's current pose
* @return true if pose was successfully retrieved
*/
bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose);
/**
* @brief Get the robot's pose as a 2D pose
* @param handle Navigation handle
* @param out_pose Output parameter with the robot's current 2D pose
* @return true if pose was successfully retrieved
*/
bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
/**
* @brief Get navigation feedback
* @param handle Navigation handle
* @param out_feedback Output parameter with navigation feedback
* @return true if feedback was successfully retrieved
* @note The feed_back_str field must be freed using nav_c_api_free_string
*/
bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback);
/**
* @brief Free navigation feedback structure
* @param feedback Feedback structure to free
*/
void navigation_free_feedback(NavFeedback* feedback);
#ifdef __cplusplus
}
#endif
#endif // NAVIGATION_C_API_H

432
src/APIs/c_api/src/nav_c_api.cpp Normal file
View File

@ -0,0 +1,432 @@
#include "nav_c_api.h"
#include <move_base_core/navigation.h>
#include <move_base_core/common.h>
#include <move_base/move_base.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <tf3/buffer_core.h>
#include <string>
#include <vector>
#include <cstring>
#include <cstdlib>
#include <boost/dll/import.hpp>
// ============================================================================
// Internal Helper Functions
// ============================================================================
namespace {
// Convert C PoseStamped to C++ PoseStamped
geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
geometry_msgs::PoseStamped cpp_pose;
cpp_pose.header.seq = c_pose->header.seq;
cpp_pose.header.stamp.sec = c_pose->header.sec;
cpp_pose.header.stamp.nsec = c_pose->header.nsec;
if (c_pose->header.frame_id) {
cpp_pose.header.frame_id = c_pose->header.frame_id;
}
cpp_pose.pose.position.x = c_pose->pose.position.x;
cpp_pose.pose.position.y = c_pose->pose.position.y;
cpp_pose.pose.position.z = c_pose->pose.position.z;
cpp_pose.pose.orientation.x = c_pose->pose.orientation.x;
cpp_pose.pose.orientation.y = c_pose->pose.orientation.y;
cpp_pose.pose.orientation.z = c_pose->pose.orientation.z;
cpp_pose.pose.orientation.w = c_pose->pose.orientation.w;
return cpp_pose;
}
// Convert C++ PoseStamped to C PoseStamped
void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
c_pose->header.seq = cpp_pose.header.seq;
c_pose->header.sec = cpp_pose.header.stamp.sec;
c_pose->header.nsec = cpp_pose.header.stamp.nsec;
if (!cpp_pose.header.frame_id.empty()) {
c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str());
} else {
c_pose->header.frame_id = nullptr;
}
c_pose->pose.position.x = cpp_pose.pose.position.x;
c_pose->pose.position.y = cpp_pose.pose.position.y;
c_pose->pose.position.z = cpp_pose.pose.position.z;
c_pose->pose.orientation.x = cpp_pose.pose.orientation.x;
c_pose->pose.orientation.y = cpp_pose.pose.orientation.y;
c_pose->pose.orientation.z = cpp_pose.pose.orientation.z;
c_pose->pose.orientation.w = cpp_pose.pose.orientation.w;
}
// Convert C++ Pose2D to C Pose2D
void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
c_pose->x = cpp_pose.x;
c_pose->y = cpp_pose.y;
c_pose->theta = cpp_pose.theta;
}
// Convert C++ NavFeedback to C NavFeedback
void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
if (!cpp_feedback.feed_back_str.empty()) {
c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str());
} else {
c_feedback->feed_back_str = nullptr;
}
c_feedback->current_pose.x = cpp_feedback.current_pose.x;
c_feedback->current_pose.y = cpp_feedback.current_pose.y;
c_feedback->current_pose.theta = cpp_feedback.current_pose.theta;
c_feedback->goal_checked = cpp_feedback.goal_checked;
c_feedback->is_ready = cpp_feedback.is_ready;
}
}
// ============================================================================
// String Management
// ============================================================================
extern "C" void nav_c_api_free_string(char* str) {
if (str) {
free(str);
}
}
// ============================================================================
// State Conversion
// ============================================================================
extern "C" char* navigation_state_to_string(NavigationState state) {
move_base_core::State cpp_state = static_cast<move_base_core::State>(static_cast<int>(state));
std::string str = move_base_core::toString(cpp_state);
return strdup(str.c_str());
}
// ============================================================================
// Helper Functions
// ============================================================================
extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta,
const char* frame_id, double offset_distance,
PoseStamped* out_goal) {
if (!out_goal || !frame_id) {
return false;
}
geometry_msgs::Pose2D pose;
pose.x = pose_x;
pose.y = pose_y;
pose.theta = pose_theta;
geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
cpp_to_c_pose_stamped(result, out_goal);
return true;
}
extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance,
PoseStamped* out_goal) {
if (!in_pose || !out_goal) {
return false;
}
geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance);
cpp_to_c_pose_stamped(result, out_goal);
return true;
}
// ============================================================================
// Navigation Handle Management
// ============================================================================
extern "C" NavigationHandle navigation_create(void) {
try {
// Create a concrete MoveBase instance
// Using default constructor - initialization will be done via navigation_initialize()
move_base::MoveBase* move_base = new move_base::MoveBase();
return static_cast<NavigationHandle>(move_base);
} catch (const std::exception& e) {
// Log error if possible (in production, use proper logging)
return nullptr;
} catch (...) {
return nullptr;
}
}
extern "C" void navigation_destroy(NavigationHandle handle) {
if (handle) {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
delete nav;
}
}
// ============================================================================
// TF Listener Management
// ============================================================================
extern "C" TFListenerHandle tf_listener_create(void) {
try {
auto tf = std::make_shared<tf3::BufferCore>();
return new std::shared_ptr<tf3::BufferCore>(tf);
} catch (...) {
return nullptr;
}
}
extern "C" void tf_listener_destroy(TFListenerHandle handle) {
if (handle) {
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(handle);
delete tf_ptr;
}
}
// ============================================================================
// Navigation Interface Methods
// ============================================================================
extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle)
{
printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__);
if (!handle || !tf_handle) {
printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__);
return false;
}
try {
printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__);
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so",
"MoveBase",
boost::dll::load_mode::append_decorations);
move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
printf("[%s:%d] Navigation created\n", __FILE__, __LINE__);
if (nav_ptr == nullptr) {
printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__);
return false;
}
nav_ptr->initialize(*tf_ptr);
return true;
} catch (const std::exception &e) {
printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what());
return false;
} catch (...) {
printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__);
return false;
}
}
extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) {
if (!handle || !points || point_count == 0) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
std::vector<geometry_msgs::Point> footprint;
footprint.reserve(point_count);
for (size_t i = 0; i < point_count; ++i) {
geometry_msgs::Point pt;
pt.x = points[i].x;
pt.y = points[i].y;
pt.z = points[i].z;
footprint.push_back(pt);
}
nav->setRobotFootprint(footprint);
return true;
} catch (...) {
return false;
}
}
extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance) {
if (!handle || !goal) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) {
return false;
}
}
extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance) {
if (!handle || !marker || !goal) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) {
return false;
}
}
extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance) {
if (!handle || !goal) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
} catch (...) {
return false;
}
}
extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal,
double yaw_goal_tolerance) {
if (!handle || !goal) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
} catch (...) {
return false;
}
}
extern "C" void navigation_pause(NavigationHandle handle) {
if (handle) {
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->pause();
} catch (...) {
// Ignore exceptions
}
}
}
extern "C" void navigation_resume(NavigationHandle handle) {
if (handle) {
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->resume();
} catch (...) {
// Ignore exceptions
}
}
}
extern "C" void navigation_cancel(NavigationHandle handle) {
if (handle) {
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->cancel();
} catch (...) {
// Ignore exceptions
}
}
}
extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
double linear_x, double linear_y, double linear_z) {
if (!handle) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 linear;
linear.x = linear_x;
linear.y = linear_y;
linear.z = linear_z;
return nav->setTwistLinear(linear);
} catch (...) {
return false;
}
}
extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
double angular_x, double angular_y, double angular_z) {
if (!handle) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 angular;
angular.x = angular_x;
angular.y = angular_y;
angular.z = angular_z;
return nav->setTwistAngular(angular);
} catch (...) {
return false;
}
}
extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) {
if (!handle || !out_pose) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_pose;
if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_stamped(cpp_pose, out_pose);
return true;
}
return false;
} catch (...) {
return false;
}
}
extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) {
if (!handle || !out_pose) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Pose2D cpp_pose;
if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_2d(cpp_pose, out_pose);
return true;
}
return false;
} catch (...) {
return false;
}
}
extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
if (!handle || !out_feedback) {
return false;
}
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
if (nav->nav_feedback_) {
cpp_to_c_nav_feedback(*nav->nav_feedback_, out_feedback);
return true;
}
return false;
} catch (...) {
return false;
}
}
extern "C" void navigation_free_feedback(NavFeedback* feedback) {
if (feedback) {
nav_c_api_free_string(feedback->feed_back_str);
feedback->feed_back_str = nullptr;
}
}

71
src/Algorithms/Cores/score_algorithm/CMakeLists.txt Normal file
View File

@ -0,0 +1,71 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(score_algorithm VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
set(PACKAGES_DIR nav_2d_msgs nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles)
find_package(Boost REQUIRED COMPONENTS filesystem system)
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo thư viện shared (.so)
add_library(score_algorithm src/score_algorithm.cpp)
target_link_libraries(score_algorithm
PUBLIC
${PACKAGES_DIR}
PRIVATE
Boost::filesystem
Boost::system
)
target_include_directories(score_algorithm PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# Cài đặt header files
install(DIRECTORY include/score_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt library
install(TARGETS score_algorithm
EXPORT score_algorithm-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT score_algorithm-targets
FILE score_algorithm-targets.cmake
DESTINATION lib/cmake/score_algorithm)
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

26
src/Algorithms/Cores/score_algorithm/include/angles/angles.h Normal file
View File

@ -0,0 +1,26 @@
#ifndef ANGLES_ANGLES_H
#define ANGLES_ANGLES_H
#include <cmath>
namespace angles
{
/**
* @brief Normalize an angle to the range [-π, π]
* @param angle The angle in radians to normalize
* @return The normalized angle in the range [-π, π]
*/
inline double normalize_angle(double angle)
{
while (angle > M_PI)
angle -= 2.0 * M_PI;
while (angle < -M_PI)
angle += 2.0 * M_PI;
return angle;
}
} // namespace angles
#endif // ANGLES_ANGLES_H

56
src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h Executable file
View File

@ -0,0 +1,56 @@
#ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__
#define _SCORE_GOAL_CHECKER_H_INCLUDED__
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/Path2D.h>
#include <memory>
namespace score_algorithm
{
/**
* @class GoalChecker
* @brief Function-object for checking whether a goal has been reached
*
* This class defines the plugin interface for determining whether you have reached
* the goal state. This primarily consists of checking the relative positions of two poses
* (which are presumed to be in the same frame). It can also check the velocity, as some
* applications require that robot be stopped to be considered as having reached the goal.
*/
class GoalChecker
{
public:
using Ptr = std::shared_ptr<score_algorithm::GoalChecker>;
virtual ~GoalChecker() {}
/**
* @brief Initialize any parameters from the NodeHandle
* @param nh NodeHandle for grabbing parameters
*/
virtual void initialize(const ros::NodeHandle& nh) = 0;
/**
* @brief Reset the state of the goal checker (if any)
*/
virtual void reset() {}
/**
* @brief Check whether the goal should be considered reached
* @param query_pose The pose to check
* @param goal_pose The pose to check against
*
* @param velocity The robot's current velocity
* @return True if goal is reached
*/
virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose,
const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D& velocity) = 0;
};
} // namespace score_algorithm
#endif // _SCORE_GOAL_CHECKER_H_INCLUDED__

169
src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h Executable file
View File

@ -0,0 +1,169 @@
#ifndef _SCORE_ALGORITHM_H_INCLUDED__
#define _SCORE_ALGORITHM_H_INCLUDED__
#include <yaml-cpp/yaml.h>
#include <iostream>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/common.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <geometry_msgs/PoseArray.h>
#include <mkt_msgs/Trajectory2D.h>
#include <score_algorithm/trajectory_generator.h>
#define FORWARD 1
#define BACKWARD -1
namespace score_algorithm
{
class ScoreAlgorithm
{
public:
using Ptr = boost::shared_ptr<score_algorithm::ScoreAlgorithm>;
inline double smoothstep(double x)
{
x = std::clamp(x, 0.0, 1.0);
return x * x * (3 - 2 * x);
}
/**
* @brief Initialize parameters as needed
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_ros Costmap pointer
*/
virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros,
const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction) = 0;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
* @param traj
*/
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
/**
* @brief Reset all data
*/
virtual void reset() {};
/**
* @brief Stoping move navigation
*/
virtual void stop() {};
/**
* @brief resume move navigation after stopped
*/
virtual void resume() {};
std::string getName()
{
return name_;
}
protected:
double sign(double x)
{
return x < 0.0 ? -1.0 : 1.0;
}
/**
* @brief Get goal index
* @param robot_pose
* @param plan
* @param distance
* @param start_index
* @param last_valid_index
* @return goal index
*/
virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<nav_2d_msgs::Pose2DStamped> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const;
/**
* @brief Get goal pose
* @param robot_pose
* @param velocity
* @param global_plan
* @param sub_goal
* @param S
* @return true if goal pose is found, false otherwise
*/
virtual bool computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S,
const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result);
/**
* @brief Find sub goal index
* @param plan
* @param index_s
* @return true if sub goal index is found, false otherwise
*/
virtual bool findSubGoalIndex(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes);
/**
* @brief Calculate journey
* @param plan
* @param start_index
* @param last_valid_index
* @return journey
*/
double journey(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
/**
* @brief Check whether the robot is stopped or not
* @param velocity The current odometry information for the robot
* @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
* @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
* @return True if the robot is stopped, false otherwise
*/
bool stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity);
void clear();
std::string name_;
TFListenerPtr tf_;
costmap_2d::Costmap2DROBOT *costmap_robot_;
TrajectoryGenerator::Ptr traj_;
int index_samples_;
bool follow_step_path_;
double xy_local_goal_tolerance_;
double old_xy_goal_tolerance_;
double angle_threshold_;
bool enable_publish_;
// ros::Publisher reached_intermediate_goals_pub_;
// ros::Publisher current_goal_pub_;
// ros::Publisher closet_robot_goal_pub_;
// ros::Publisher transformed_plan_pub_, compute_plan_pub_;
std::vector<nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
std::vector<unsigned int> start_index_saved_vt_;
unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
unsigned int sub_start_index_saved_, sub_start_seq_saved_;
};
} // namespace score_algorithm
#endif // SCORE_ALGORITHM_H_INCLUDED__

136
src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h Executable file
View File

@ -0,0 +1,136 @@
#ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
#define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
#include <yaml-cpp/yaml.h>
#include <mkt_msgs/Trajectory2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <memory>
#include <vector>
#include <geometry_msgs/Vector3.h>
namespace score_algorithm
{
/**
* @class TrajectoryGenerator
* @brief Interface for iterating through possible velocities and creating trajectories
*
* This class defines the plugin interface for two separate but related components.
*
* First, this class provides an iterator interface for exploring all of the velocities
* to search, given the current velocity.
*
* Second, the class gives an independent interface for creating a trajectory from a twist,
* i.e. projecting it out in time and space.
*
* Both components rely heavily on the robot's kinematic model, and can share many parameters,
* which is why they are grouped into a singular class.
*/
class TrajectoryGenerator
{
public:
using Ptr = boost::shared_ptr<score_algorithm::TrajectoryGenerator>;
virtual ~TrajectoryGenerator() {}
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const YAML::Node &nh) = 0;
/**
* @brief Reset the state (if any) when the planner gets a new goal
*/
virtual void reset() {}
/**
* @brief Start a new iteration based on the current velocity
* @param current_velocity
*/
virtual void startNewIteration(const nav_2d_msgs::Twist2D &current_velocity) = 0;
/**
* @brief Set direct of robot based on the current velocity
* @param direct
*/
virtual void setDirect(int *xytheta_direct) = 0;
/**
* @brief set velocity for x, y asix of the robot
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) = 0;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) = 0;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
/**
* @brief Test to see whether there are more twists to test
* @return True if more twists, false otherwise
*/
virtual bool hasMoreTwists() = 0;
/**
* @brief Return the next twist and advance the iteration
* @return The Twist!
*/
virtual nav_2d_msgs::Twist2D nextTwist() = 0;
/**
* @brief Get all the twists for an iteration.
*
* Note: Resets the iterator if one is in process
*
* @param current_velocity
* @return all the twists
*/
virtual std::vector<nav_2d_msgs::Twist2D> getTwists(const nav_2d_msgs::Twist2D &current_velocity)
{
std::vector<nav_2d_msgs::Twist2D> twists;
startNewIteration(current_velocity);
while (hasMoreTwists())
{
twists.push_back(nextTwist());
}
return twists;
}
/**
* @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
* @param start_pose Current robot location
* @param start_vel Current robot velocity
* @param cmd_vel The desired command velocity
*/
virtual mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose,
const nav_2d_msgs::Path2D &transformed_plan,
const nav_2d_msgs::Twist2D &start_vel,
const nav_2d_msgs::Twist2D &cmd_vel) = 0;
virtual YAML::Node getNodeHandle() = 0;
};
} // namespace score_algorithm
#endif // _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H

470
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp Normal file
View File

@ -0,0 +1,470 @@
#include <score_algorithm/score_algorithm.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/conversions.h>
#include <angles/angles.h>
unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<nav_2d_msgs::Pose2DStamped> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index + 1;
const double start_direction_x = plan[goal_index].pose.x - plan[start_index].pose.x;
const double start_direction_y = plan[goal_index].pose.y - plan[start_index].pose.y;
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
{ // make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2));
// double angle_threshold = 0.0;
for (unsigned int i = start_index + 1; i <= last_valid_index; i++)
{
const double current_direction_x = plan[i].pose.x - plan[i - 1].pose.x;
const double current_direction_y = plan[i].pose.y - plan[i - 1].pose.y;
const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2));
S += dd;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
goal_index = i;
if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance)
{
break;
}
}
}
}
if (last_valid_index - goal_index == 1)
goal_index = last_valid_index;
return goal_index;
}
bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes)
{
if (plan.empty())
{
return false;
}
double x_direction_saved = 0.0;
geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = robot::Time::now();
poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
nav_2d_msgs::Pose2DStamped p1, p2, p3;
if (plan.size() < 3)
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
}
else
{
unsigned int i = 0;
p1 = plan[i];
for (i = 0; i < (unsigned int)plan.size() - 2; i++)
{
double egde1_angle, egde2_angle;
p2 = plan[i + 1];
if ((fabs(p2.pose.y - p1.pose.y) > 1e-9 || fabs(p2.pose.x - p1.pose.x) > 1e-9))
{
egde1_angle = atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x);
}
else
continue;
const double target_direction = cos(fabs(egde1_angle - p2.pose.theta));
if (target_direction * x_direction_saved < 0.0)
{
if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end())
{
seq_s.push_back(plan[i].header.seq);
mutes.push_back(plan[i]);
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
poseArrayMsg.poses.push_back(pose);
}
}
x_direction_saved = target_direction;
p3 = plan[i + 2];
if ((fabs(p3.pose.y - p2.pose.y) > 1e-9 || fabs(p3.pose.x - p2.pose.x) > 1e-9))
{
egde2_angle = atan2(p3.pose.y - p2.pose.y, p3.pose.x - p2.pose.x);
p1 = p2;
}
else
continue;
double thresh_angle = 0.3;
double angle = fabs(remainder(egde1_angle - egde2_angle, 2 * M_PI));
if (angle >= thresh_angle || angle >= M_PI - thresh_angle)
{
if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end())
{
seq_s.push_back(plan[i + 1].header.seq);
mutes.push_back(plan[i + 1]);
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
poseArrayMsg.poses.push_back(pose);
}
p2 = p3;
}
}
}
if (seq_s.empty())
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
else
{
if (seq_s.back() < plan.back().header.seq)
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
}
return true;
}
bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S,
const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result)
{
result.poses.clear();
if (global_plan.poses.empty())
{
std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl;
return false;
}
std::vector<unsigned int> seq_s;
std::vector<nav_2d_msgs::Pose2DStamped> mutes;
if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes))
{
std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl;
return false;
}
unsigned int sub_goal_index;
unsigned int sub_start_index;
bool found_sub_goal_index_saved = false, found_sub_start_index_saved = false;
for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i)
{
if (global_plan.poses[i].header.seq == sub_goal_seq_saved_)
{
sub_goal_index_saved_ = i;
found_sub_goal_index_saved = true;
}
if (global_plan.poses[i].header.seq == sub_start_seq_saved_)
{
sub_start_index_saved_ = i;
found_sub_start_index_saved = true;
}
if (found_sub_goal_index_saved && found_sub_start_index_saved)
{
break;
}
}
sub_goal_index = !found_sub_goal_index_saved ? (unsigned int)global_plan.poses.size() : sub_goal_index_saved_;
sub_start_index = !found_sub_start_index_saved ? 0 : sub_start_index_saved_;
std::vector<unsigned int> index_s;
for (auto seq : seq_s)
{
for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i)
{
if (global_plan.poses[i].header.seq == seq)
{
index_s.push_back(i);
break;
}
}
}
// Handle empty seq_s
if (index_s.empty())
{
sub_goal_index = (unsigned int)global_plan.poses.size();
sub_goal_seq_saved_ = global_plan.poses.back().header.seq;
std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl;
}
else
{
// Update sub_goal_index if index_s.front() is greater and valid
if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size())
{
sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front();
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl;
}
// Process index_s with multiple elements
if (index_s.size() > 1)
{
for (size_t i = 0; i < index_s.size(); ++i)
{
if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size())
{
std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl;
continue;
}
double dx = robot_pose.pose.x - global_plan.poses[index_s[i - 1]].pose.x;
double dy = robot_pose.pose.y - global_plan.poses[index_s[i - 1]].pose.y;
double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[index_s[i - 1]].pose.theta);
double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy;
if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
{
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
if (fabs(tolerance) <= xy_local_goal_tolerance_)
{
// ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta);
// ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
if (index_s[i] > sub_goal_index_saved_)
{
// ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size());
// ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size());
sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1;
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
old_xy_goal_tolerance_ = 0.0;
if (follow_step_path_)
{
sub_start_index = index_s[i - 1];
sub_start_seq_saved_ = global_plan.poses[sub_start_index].header.seq;
}
break;
}
}
}
else
old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
}
}
else
{
// Validate sub_goal_index
if (sub_goal_index >= (unsigned int)global_plan.poses.size() - 1)
{
sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
}
else
{
double dx = robot_pose.pose.x - global_plan.poses[sub_goal_index].pose.x;
double dy = robot_pose.pose.y - global_plan.poses[sub_goal_index].pose.y;
double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta);
double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy;
double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy;
if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1)
{
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
if (fabs(tolerance) <= xy_local_goal_tolerance_)
{
// ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta);
// ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
old_xy_goal_tolerance_ = 0.0;
std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl;
}
}
else
old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
}
}
}
if (sub_start_index >= sub_goal_index)
{
std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl;
return false;
}
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
// Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi
unsigned int closet_index = 0;
double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
bool started_path = false;
for (unsigned int i = sub_start_index; i < sub_goal_index; ++i)
{
// Still on the costmap. Continue.
double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[i].pose, robot_pose.pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
if (distance_to_start > distance)
{
start_index = i;
closet_index = start_index;
distance_to_start = distance;
started_path = true;
}
}
// Nếu khồng tìm được điểm gần với robot nhất thì trả về false
if (!started_path)
{
std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl;
return false;
}
std::stringstream ss;
start_index_saved_vt_.push_back(global_plan.poses[start_index].header.seq);
if (start_index_saved_vt_.size() > index_samples_)
start_index_saved_vt_.erase(start_index_saved_vt_.begin());
unsigned num = 0;
if (!start_index_saved_vt_.empty())
{
for (int i = 0; i < (int)start_index_saved_vt_.size() - 1; i++)
{
if (start_index_saved_vt_[i] == start_index_saved_vt_[i + 1])
num++;
ss << start_index_saved_vt_[i] << " ";
}
}
if (num < index_samples_ - 1 && !start_index_saved_vt_.empty())
{
auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end()));
unsigned int max_start_index = start_index;
for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); i++)
{
if (global_plan.poses[i].header.seq == max_iter)
{
max_start_index = i;
break;
}
}
double S1 = journey(global_plan.poses, start_index, (unsigned int)global_plan.poses.size() - 1);
double S2 = journey(global_plan.poses, max_start_index, (unsigned int)global_plan.poses.size() - 1);
if (S2 - S1)
{
start_index = max_start_index;
}
}
// ROS_INFO_STREAM_THROTTLE(0.1, ss.str() << "\n");
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
if (start_index < 1)
start_index = 1;
// unsigned int last_valid_index = (unsigned int)global_plan.poses.size() - 1;
unsigned int last_valid_index = sub_goal_index;
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
// Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
unsigned int goal_index = start_index;
while (goal_index < last_valid_index)
{
// Tìm kiếm vị trí điểm mục tiêu phù hợp
goal_index = getGoalIndex(robot_pose.pose, global_plan.poses, S, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
// geometry_msgs::PoseArray poseArrayMsg;
// poseArrayMsg.header.stamp = ros::Time::now();
// poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
// int c = 0;
for (auto &reached_intermediate_goal : reached_intermediate_goals_)
{
double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
// geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
// poseArrayMsg.poses.push_back(pose);
if (distance < xy_local_goal_tolerance_)
{
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index < last_valid_index; ++start_index)
{
distance = nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[start_index].pose);
if (distance >= xy_local_goal_tolerance_)
break;
}
break;
}
}
if (!goal_already_reached)
{
// new goal not in reached_intermediate_goals_
double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[start_index].pose, global_plan.poses[goal_index].pose));
if (distance < xy_local_goal_tolerance_)
{
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(global_plan.poses[goal_index]);
std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl;
}
else
{
// we've found a new goal!
break;
}
}
}
if (start_index > goal_index)
start_index = goal_index;
if (goal_index > last_valid_index)
goal_index = last_valid_index;
if ((unsigned int)global_plan.poses.size() == 2 && !global_plan.poses.empty())
{
start_index = 0;
goal_index = 1;
}
if (goal_index == (unsigned int)global_plan.poses.size() - 1)
start_index = (unsigned int)global_plan.poses.size() - 2;
start_index_global_plan = start_index;
goal_index_global_plan = goal_index;
nav_2d_msgs::Pose2DStamped sub_pose;
sub_pose = global_plan.poses[closet_index];
nav_2d_msgs::Pose2DStamped sub_goal;
sub_goal = global_plan.poses[goal_index];
result.header = global_plan.header;
for (int i = closet_index; i <= goal_index; i++)
{
result.poses.push_back(global_plan.poses[i]);
}
return true;
}
double score_algorithm::ScoreAlgorithm::journey(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
{
double S = 0;
if (last_valid_index - start_index > 1)
{
for (int i = start_index; i < last_valid_index; i++)
{
S += nav_2d_utils::poseDistance(plan[i].pose, plan[i + 1].pose);
}
}
return S;
}
bool score_algorithm::ScoreAlgorithm::stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
{
return fabs(velocity.theta) <= rot_stopped_velocity && fabs(velocity.x) <= trans_stopped_velocity && fabs(velocity.y) <= trans_stopped_velocity;
}
void score_algorithm::ScoreAlgorithm::clear()
{
reached_intermediate_goals_.clear();
start_index_saved_vt_.clear();
sub_goal_index_saved_ = sub_goal_seq_saved_ = 0;
sub_start_index_saved_, sub_start_seq_saved_ = 0;
}

34
src/Algorithms/Libraries/angles/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 3.10)
project(angles VERSION 1.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Create interface library (header-only)
add_library(angles INTERFACE)
# Set include directories
target_include_directories(angles INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Install headers
install(DIRECTORY include/angles
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# Install target
install(TARGETS angles
EXPORT angles-targets
INCLUDES DESTINATION include
)
# Export targets
install(EXPORT angles-targets
FILE angles-targets.cmake
DESTINATION lib/cmake/angles
)

26
src/Algorithms/Libraries/angles/include/angles/angles.h Normal file
View File

@ -0,0 +1,26 @@
#ifndef ANGLES_ANGLES_H
#define ANGLES_ANGLES_H
#include <cmath>
namespace angles
{
/**
* @brief Normalize an angle to the range [-π, π]
* @param angle The angle in radians to normalize
* @return The normalized angle in the range [-π, π]
*/
inline double normalize_angle(double angle)
{
while (angle > M_PI)
angle -= 2.0 * M_PI;
while (angle < -M_PI)
angle += 2.0 * M_PI;
return angle;
}
} // namespace angles
#endif // ANGLES_ANGLES_H

92
src/Algorithms/Libraries/kalman/CMakeLists.txt Executable file
View File

@ -0,0 +1,92 @@
cmake_minimum_required(VERSION 3.10)
project(kalman VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# Find dependencies
find_package(Eigen3 REQUIRED)
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIRS}
)
# Tìm tất cả file source
file(GLOB SOURCES "src/kalman.cpp")
file(GLOB HEADERS "include/kalman/kalman.h")
# Tạo thư viện shared (.so)
add_library(kalman SHARED ${SOURCES} ${HEADERS})
# Link libraries
target_link_libraries(kalman
PUBLIC
Eigen3::Eigen
)
# Set include directories
target_include_directories(kalman
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${EIGEN3_INCLUDE_DIRS}
)
# Tạo executable cho test
add_executable(kalman_node src/kalman-test.cpp)
target_link_libraries(kalman_node
PRIVATE
kalman
Eigen3::Eigen
)
# Cài đặt header files
install(DIRECTORY include/kalman
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# Cài đặt library
install(TARGETS kalman
EXPORT kalman-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)
# Cài đặt executable (optional)
install(TARGETS kalman_node
RUNTIME DESTINATION bin
)
# Export targets
install(EXPORT kalman-targets
FILE kalman-targets.cmake
DESTINATION lib/cmake/kalman
)
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Eigen3 found: ${EIGEN3_FOUND}")
message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIRS}")
message(STATUS "=================================")

18
src/Algorithms/Libraries/kalman/README.md Executable file
View File

@ -0,0 +1,18 @@
Kalman Filter
=============
This is a basic Kalman filter implementation in C++ using the
[Eigen](http://eigen.tuxfamily.org/) library. It implements the algorithm
directly as found in [An Introduction to the Kalman Filter](http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf).
There is a test program that estimates the motion of a projectile based on
noisy observations. To run it, use CMake:
cd kalman-cpp
mkdir build
cd build
cmake ..
make
./kalman-test
Note: You may have to specify the path to your Eigen library in
`CMakeLists.txt`.

91
src/Algorithms/Libraries/kalman/include/kalman/kalman.h Executable file
View File

@ -0,0 +1,91 @@
/**
* Kalman filter implementation using Eigen. Based on the following
* introductory paper:
*
* http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <Eigen/Dense>
#pragma once
class KalmanFilter {
public:
/**
* Create a Kalman filter with the specified matrices.
* A - System dynamics matrix
* C - Output matrix
* Q - Process noise covariance
* R - Measurement noise covariance
* P - Estimate error covariance
*/
KalmanFilter(
double dt,
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& P
);
/**
* Create a blank estimator.
*/
KalmanFilter();
/**
* Initialize the filter with initial states as zero.
*/
void init();
/**
* Initialize the filter with a guess for initial states.
*/
void init(double t0, const Eigen::VectorXd& x0);
/**
* Update the estimated state based on measured values. The
* time step is assumed to remain constant.
*/
void update(const Eigen::VectorXd& y);
/**
* Update the estimated state based on measured values,
* using the given time step and dynamics matrix.
*/
void update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A);
/**
* Return the current state and time.
*/
Eigen::VectorXd state() { return x_hat; };
double time() { return t; };
private:
// Matrices for computation
Eigen::MatrixXd A, C, Q, R, P, K, P0;
// System dimensions
int m, n;
// Initial and current time
double t0, t;
// Discrete time step
double dt;
// Is the filter initialized?
bool initialized;
// n-size identity
Eigen::MatrixXd I;
// Estimated states
Eigen::VectorXd x_hat, x_hat_new;
};

77
src/Algorithms/Libraries/kalman/src/kalman-test.cpp Executable file
View File

@ -0,0 +1,77 @@
/**
* Test for the KalmanFilter class with 1D projectile motion.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <kalman/kalman.h>
int main(int argc, char* argv[]) {
int n = 3; // Number of states
int m = 1; // Number of measurements
double dt = 1.0/30; // Time step
Eigen::MatrixXd A(n, n); // System dynamics matrix
Eigen::MatrixXd C(m, n); // Output matrix
Eigen::MatrixXd Q(n, n); // Process noise covariance
Eigen::MatrixXd R(m, m); // Measurement noise covariance
Eigen::MatrixXd P(n, n); // Estimate error covariance
// Discrete LTI projectile motion, measuring position only
A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
C << 1, 0, 0;
// Reasonable covariance matrices
Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
R << 5;
P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
std::cout << "A: \n" << A << std::endl;
std::cout << "C: \n" << C << std::endl;
std::cout << "Q: \n" << Q << std::endl;
std::cout << "R: \n" << R << std::endl;
std::cout << "P: \n" << P << std::endl;
// Construct the filter
KalmanFilter kf(dt,A, C, Q, R, P);
// List of noisy position measurements (y)
std::vector<double> measurements = {
1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,
1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,
2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,
2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,
2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,
2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,
2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,
1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,
0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813
};
// Best guess of initial states
Eigen::VectorXd x0(n);
double t = 0;
x0 << measurements[0], 0, -9.81;
kf.init(t, x0);
// Feed measurements into filter, output estimated states
Eigen::VectorXd y(m);
std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl;
for(int i = 0; i < measurements.size(); i++) {
t += dt;
y << measurements[i];
kf.update(y);
std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose()
<< ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl;
}
return 0;
}

65
src/Algorithms/Libraries/kalman/src/kalman.cpp Executable file
View File

@ -0,0 +1,65 @@
/**
* Implementation of KalmanFilter class.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <iostream>
#include <stdexcept>
#include <kalman/kalman.h>
KalmanFilter::KalmanFilter(
double dt,
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C,
const Eigen::MatrixXd& Q,
const Eigen::MatrixXd& R,
const Eigen::MatrixXd& P)
: A(A), C(C), Q(Q), R(R), P0(P),
m(C.rows()), n(A.rows()), dt(dt), initialized(false),
I(n, n), x_hat(n), x_hat_new(n)
{
I.setIdentity();
}
KalmanFilter::KalmanFilter() {}
void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
x_hat = x0;
P = P0;
this->t0 = t0;
t = t0;
initialized = true;
}
void KalmanFilter::init() {
x_hat.setZero();
P = P0;
t0 = 0;
t = t0;
initialized = true;
}
void KalmanFilter::update(const Eigen::VectorXd& y) {
if(!initialized)
throw std::runtime_error("Filter is not initialized!");
x_hat_new = A * x_hat;
P = A*P*A.transpose() + Q;
K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
x_hat_new += K * (y - C*x_hat_new);
P = (I - K*C)*P;
x_hat = x_hat_new;
t += dt;
}
void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {
this->A = A;
this->dt = dt;
update(y);
}

110
src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt Normal file
View File

@ -0,0 +1,110 @@
cmake_minimum_required(VERSION 3.10)
project(mkt_algorithm VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# Find dependencies
find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIRS}
)
# Dependencies packages
set(PACKAGES_DIR
geometry_msgs
score_algorithm
nav_2d_msgs
nav_2d_utils
kalman
angles
nav_grid
costmap_2d
sensor_msgs
visualization_msgs
std_msgs
)
# Tìm tất cả file source cho diff library
file(GLOB DIFF_SOURCES
"src/diff/diff_predictive_trajectory.cpp"
"src/diff/diff_rotate_to_goal.cpp"
"src/diff/diff_go_straight.cpp"
)
file(GLOB DIFF_HEADERS
"include/mkt_algorithm/diff/*.h"
)
# Tạo thư viện shared cho diff
add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
# Link libraries
target_link_libraries(mkt_algorithm_diff
PUBLIC
${PACKAGES_DIR}
Boost::system
Eigen3::Eigen
)
# Set include directories
target_include_directories(mkt_algorithm_diff
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${EIGEN3_INCLUDE_DIRS}
)
# Cài đặt header files
install(DIRECTORY include/mkt_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# Cài đặt library
install(TARGETS mkt_algorithm_diff
EXPORT mkt_algorithm-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)
# Cài đặt plugins.xml (nếu cần cho pluginlib)
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
install(FILES plugins.xml
DESTINATION share/${PROJECT_NAME}
)
endif()
# Export targets
install(EXPORT mkt_algorithm-targets
FILE mkt_algorithm-targets.cmake
DESTINATION lib/cmake/mkt_algorithm
)
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Eigen3 found: ${EIGEN3_FOUND}")
message(STATUS "=================================")

0
src/Algorithms/Libraries/mkt_algorithm/README.md Normal file
View File

108
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h Normal file
View File

@ -0,0 +1,108 @@
#ifndef _NAV_ALGORITHM_BICYCLE_H_INCLUDED__
#define _NAV_ALGORITHM_BICYCLE_H_INCLUDED__
#include <score_algorithm/score_algorithm.h>
#include <mkt_algorithm/equation_line.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <kalman/kalman.h>
namespace mkt_algorithm
{
/**
* @class Bicycle
* @brief Standard Bicycle-like ScoreAlgorithm
*/
class Bicycle : public score_algorithm::ScoreAlgorithm
{
public:
Bicycle() {}; //: line_generator_(nullptr) {};
virtual ~Bicycle() {};
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
*/
virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
protected:
unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const;
double journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity,
const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal,
double &target_direction);
bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
ros::NodeHandle nh_priv_, nh_;
std::string frame_id_path_;
ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
geometry_msgs::Pose2D rev_sub_goal_, sub_goal_;
geometry_msgs::Pose2D goal_;
nav_2d_msgs::Path2D global_plan_;
unsigned int start_index_saved_;
unsigned int sub_goal_index_saved_, sub_start_index_saved_;
std::vector<unsigned int> start_index_saved_vt_;
int index_samples_;
double steering_fix_wheel_distance_x_;
double steering_fix_wheel_distance_y_;
double xy_local_goal_tolerance_;
double xy_goal_tolerance_;
double angle_threshold_;
double acc_lim_theta_;
double decel_lim_theta_;
double x_direction_, y_direction_, theta_direction_;
double k_d_reduce_, distance_rg_, k_ld_, ld_min_, ld_max_;
double controller_frequency_param_name_;
bool follow_step_path_;
bool is_filter_;
boost::shared_ptr<KalmanFilter> kf_;
ros::Publisher cmd_raw_pub_;
ros::Time last_actuator_update_;
}; // class Bicycle
} // namespace mkt_algorithm
#endif //_NAV_ALGORITHM_BICYCLE_H_INCLUDED__

74
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h Normal file
View File

@ -0,0 +1,74 @@
#ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <iostream>
namespace mkt_algorithm
{
/**
* @class NoSolution
*/
class NoSolution : public std::exception {
public:
const char * what () const throw () {
return "Robot cannot calculate error line!";
}
}; // class NoSolution
/**
* @class LineGenerator
*/
class LineGenerator
{
/* data */
struct LineEquation {
geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
float a, b, c ; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */
};
/* Save data present position */
struct PresentPosition {
float Gxh, Gyh; /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot */
float Yaw_degree; /* Angle of Robot (degree) */
float Yaw_rad; /* Angle of Robot (degree) */
float error_line; /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2) */
double distance_to_goal; /* The distance from query pose to goal */
double distance_to_start; /* The distance from query pose to start*/
double distace_start_goal;
};
public:
/**
* @brief To caculated the equation of the line
* @param start_point start point of the line
* @param end_point end point of the line
*/
virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
/**
* @brief Calculating error
* @param[in] Lm The distance from the center of the wheel axis to the center line
* @param[in] pose
* @param[in] rev
*/
virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
/**
* @brief Calculating tolerance
* @param query_pose The pose to check
* @param goal_pose The pose to check against
*/
virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
/* Properties */
LineEquation Leq_;
PresentPosition present_pose_;
}; // class LineGenerator
} // namespace mkt_algorithm
#endif // __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__

70
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
#include <score_algorithm/score_algorithm.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
namespace mkt_algorithm
{
/**
* @class GoStraight
* @brief Standard Bicycle-like ScoreAlgorithm
*/
class GoStraight : public score_algorithm::ScoreAlgorithm
{
public:
GoStraight() {};
virtual ~GoStraight() {};
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
*/
virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
protected:
ros::NodeHandle nh_;
ros::NodeHandle nh_kinematics_;
double steering_fix_wheel_distance_x_;
double steering_fix_wheel_distance_y_;
geometry_msgs::Pose2D goal_;
ros::Time time_last_cmd_;
}; // class GoStraight
} // namespace mkt_algorithm
#endif

45
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h Normal file
View File

@ -0,0 +1,45 @@
#ifndef _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__
#include <score_algorithm/goal_checker.h>
#include <mkt_algorithm/equation_line.h>
namespace mkt_algorithm
{
class GoalChecker : public score_algorithm::GoalChecker
{
public:
/**
* @brief Initialize any parameters from the NodeHandle
* @param nh NodeHandle for grabbing parameters
*/
virtual void initialize(const ros::NodeHandle& nh) override;
/**
* @brief Reset the state of the goal checker (if any)
*/
virtual void reset() override;
/**
* @brief Check whether the goal should be considered reached
* @param query_pose The pose to check
* @param goal_pose The pose to check against
* @param velocity The robot's current velocity
* @return True if goal is reached
*/
bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
const std::vector<geometry_msgs::PoseStamped>& path, const nav_2d_msgs::Twist2D& velocity) override;
protected:
boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
double yaw_goal_tolerance_;
double xy_goal_tolerance_;
double old_xy_goal_tolerance_;
bool setup_;
};
} // namespace mkt_algorithm
#endif // _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__

60
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h Normal file
View File

@ -0,0 +1,60 @@
#ifndef _NAV_ALGORITHM_PTA_H_INCLUDED__
#define _NAV_ALGORITHM_PTA_H_INCLUDED__
#include <score_algorithm/score_algorithm.h>
#include <mkt_algorithm/equation_line.h>
namespace mkt_algorithm
{
/**
* @class PTA
* @brief Standard PTA-like ScoreAlgorithm
*/
class PTA : public score_algorithm::ScoreAlgorithm
{
public:
PTA() : line_generator_(nullptr) {};
virtual ~PTA() {};
// Standard ScoreAlgorithm Interface
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
*/
virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
protected:
boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
double pta_amplitude_max_; // (Default: 0.5 - Range: 0.1 -> 1.0) Cai dat bien do (mot phia - m) cho phep robot di chuyen trong do
}; // class PTA
} // namespace mkt_algorithm
#endif //_NAV_ALGORITHM_PTA_H_INCLUDED__

74
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h Normal file
View File

@ -0,0 +1,74 @@
#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
#include <score_algorithm/score_algorithm.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
namespace mkt_algorithm
{
/**
* @class GoStraight
* @brief Standard Bicycle-like ScoreAlgorithm
*/
class RotateToGoal : public score_algorithm::ScoreAlgorithm
{
public:
RotateToGoal() {};
virtual ~RotateToGoal() {};
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
*/
virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override;
protected:
ros::NodeHandle nh_;
ros::NodeHandle nh_kinematics_;
geometry_msgs::Pose2D sub_goal_;
double steering_fix_wheel_distance_x_;
double steering_fix_wheel_distance_y_;
double angle_threshold_;
double current_direction_;
double velocity_rotate_min_, velocity_rotate_max_;
geometry_msgs::Pose2D goal_;
ros::Time time_last_cmd_;
}; // class RotateToGoal
} // namespace mkt_algorithm
#endif

43
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h Normal file
View File

@ -0,0 +1,43 @@
#ifndef _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
namespace mkt_algorithm
{
namespace diff
{
/**
* @class GoStraight
* @brief Standard Bicycle-like ScoreAlgorithm
*/
class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory
{
public:
GoStraight() {};
virtual ~GoStraight() {};
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
* @param traj
*/
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
}; // class GoStraight
} // namespace diff
} // namespace mkt_algorithm
#endif // _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__

325
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h Normal file
View File

@ -0,0 +1,325 @@
#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#include <score_algorithm/score_algorithm.h>
#include <boost/dll/import.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
// #include <visualization_msgs/Marker.h>
// #include <ddynamic_reconfigure/ddynamic_reconfigure.h>
// #include <base_local_planner/costmap_model.h>
// #include <base_local_planner/world_model.h>
// #include <base_local_planner/trajectory_planner.h>
// #include <base_local_planner/map_grid_visualizer.h>
// #include <base_local_planner/BaseLocalPlannerConfig.h>
// #include <teb_local_planner/pose_se2.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
#include <tf3/utils.h>
#include <nav_msgs/Path.h>
#include <kalman/kalman.h>
#include <vector>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/conversions.h>
namespace mkt_algorithm
{
namespace diff
{
/**
* @class PredictiveTrajectory
* @brief Standard PredictiveTrajectory-like ScoreAlgorithm
*/
class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
{
public:
PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
virtual ~PredictiveTrajectory();
// Standard ScoreAlgorithm Interface
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction) override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
* @param traj
*/
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Stoping move navigation
*/
virtual void stop() override;
/**
* @brief resume move navigation after stopped
*/
virtual void resume() override;
protected:
inline double sign(double x)
{
return x < 0.0 ? -1.0 : 1.0;
}
/**
* @brief Initialize parameters
*/
virtual void getParams();
/**
* @brief Initialize dynamic reconfigure
* @param nh NodeHandle to read parameters from
*/
virtual void initDynamicReconfigure(const ros::NodeHandle &nh);
/**
* @brief Dynamically adjust look ahead distance based on the speed
* @param velocity
* @return look ahead distance
*/
double getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity);
/**
* @brief Get lookahead point on the global plan
* @param lookahead_dist
* @param global_plan
* @return lookahead point
*/
std::vector<nav_2d_msgs::Pose2DStamped>::iterator
getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan);
/**
* @brief Create carrot message
* @param carrot_pose
* @return carrot message
*/
geometry_msgs::PointStamped createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose);
/**
* @brief Prune global plan
* @param tf
* @param pose
* @param global_plan
* @param dist_behind_robot
* @return true if plan is pruned, false otherwise
*/
bool pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose,
nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
*
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
* such that the index of the current goal pose is returned as well as
* the transformation between the global plan and the planning frame.
* @param tf A reference to a tf buffer
* @param global_plan The plan to be transformed
* @param pose The pose of the robot
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
* @param[out] transformed_plan Populated with the transformed plan
* @return \c true if the global plan is transformed, \c false otherwise
*/
bool transformGlobalPlan(
TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
nav_2d_msgs::Path2D &transformed_plan);
/**
* @brief Should rotate to path
* @param carrot_pose
* @param angle_to_path
* @return true if should rotate, false otherwise
*/
bool shouldRotateToPath(
const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);
/**
* @brief Rotate to heading
* @param angle_to_path
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands to be filled
*/
void rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel);
/**
* @brief the robot is moving acceleration limits
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands
* @param cmd_vel_result The velocity commands result
*/
void moveWithAccLimits(
const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result);
/**
* @brief Stop the robot taking into account acceleration limits
* @param pose The pose of the robot in the global frame
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel);
/**
* @brief Apply constraints
* @param dist_error
* @param lookahead_dist
* @param curvature
* @param curr_speed
* @param pose_cost
* @param linear_vel
* @param sign
*/
void applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const nav_2d_msgs::Twist2D &curr_speed,
const double &pose_cost, double &linear_vel, double &sign_x);
std::vector<geometry_msgs::Point> interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution);
/**
* @brief Cost at pose
* @param x
* @param y
* @return cost
*/
double costAtPose(const double &x, const double &y);
void updateCostAtOffset(
TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose,
double x_offset, double y_offset, double &cost_left, double &cost_right);
double computeDecelerationFactor(double remaining_distance, double decel_distance);
double getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle,
double amplitude_threshold = 0.3, size_t window_size = 20);
void publishMarkers(nav_2d_msgs::Pose2DStamped pose);
std::vector<double> angle_history_;
size_t window_size_;
bool initialized_;
bool nav_stop_, avoid_obstacles_;
ros::NodeHandle nh_, nh_priv_;
std::string frame_id_path_;
std::string robot_base_frame_;
ros::Publisher carrot_pub_;
nav_2d_msgs::Pose2DStamped goal_;
nav_2d_msgs::Path2D global_plan_;
nav_2d_msgs::Path2D compute_plan_;
nav_2d_msgs::Path2D transform_plan_;
nav_2d_msgs::Twist2D prevous_drive_cmd_;
ros::Publisher drive_pub_;
double x_direction_, y_direction_, theta_direction_;
double max_robot_pose_search_dist_;
double global_plan_prune_distance_{1.0};
// Lookahead
bool use_velocity_scaled_lookahead_dist_;
double lookahead_time_;
double lookahead_dist_;
double min_lookahead_dist_;
double max_lookahead_dist_;
// journey
double min_journey_squared_{1e9};
double max_journey_squared_{1e9};
// Rotate to heading
bool use_rotate_to_heading_;
double rotate_to_heading_min_angle_;
double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
double min_path_distance_, max_path_distance_;
// Regulated linear velocity scaling
bool use_regulated_linear_velocity_scaling_;
double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
double rot_stopped_velocity_, trans_stopped_velocity_;
double min_approach_linear_velocity_;
double regulated_linear_scaling_min_radius_;
double regulated_linear_scaling_min_speed_;
bool use_cost_regulated_linear_velocity_scaling_;
double inflation_cost_scaling_factor_;
double cost_scaling_dist_, cost_scaling_gain_;
double cost_left_goal_, cost_right_goal_;
double cost_left_side_ , cost_right_side_;
double center_cost_;
// Control frequency
double control_duration_;
std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddr_;
base_local_planner::BaseLocalPlannerConfig config_;
base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use
base_local_planner::TrajectoryPlanner *tc_; ///< @brief The trajectory controller
base_local_planner::MapGridVisualizer map_viz_;
std::vector<geometry_msgs::Point> footprint_spec_;
ros::Time last_actuator_update_;
boost::shared_ptr<KalmanFilter> kf_;
int m_, n_;
Eigen::MatrixXd A;
Eigen::MatrixXd C;
Eigen::MatrixXd Q;
Eigen::MatrixXd R;
Eigen::MatrixXd P;
ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_;
// visualization_msgs::Marker L_, R_;
}; // class PredictiveTrajectory
} // namespace diff
} // namespace mkt_algorithm
#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

68
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h Normal file
View File

@ -0,0 +1,68 @@
#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__
#define _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
namespace mkt_algorithm
{
namespace diff
{
/**
* @class GoStraight
* @brief Standard Bicycle-like ScoreAlgorithm
*/
class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
{
public:
RotateToGoal() {};
virtual ~RotateToGoal() {};
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
* @param traj
*/
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
protected:
/**
* @brief Initialize parameters
*/
virtual void getParams() override;
}; // class RotateToGoalDiff
} // namespace diff
} // namespace mkt_algorithm
#endif // _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__

94
src/Algorithms/Libraries/mkt_algorithm/package.xml Normal file
View File

@ -0,0 +1,94 @@
<?xml version="1.0"?>
<package format="2">
<name>mkt_algorithm</name>
<version>0.0.0</version>
<description>The mkt_algorithm package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/mkt_algorithm</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>score_algorithm</build_depend>
<build_depend>nav_2d_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_2d_utils</build_depend>
<build_depend>kalman</build_depend>
<build_depend>ddynamic_reconfigure</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>base_local_planner</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>score_algorithm</build_export_depend>
<build_export_depend>nav_2d_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>nav_2d_utils</build_export_depend>
<build_export_depend>kalman</build_export_depend>
<build_export_depend>ddynamic_reconfigure</build_export_depend>
<build_export_depend>costmap_2d</build_export_depend>
<build_export_depend>base_local_planner</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>score_algorithm</exec_depend>
<exec_depend>nav_2d_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>nav_2d_utils</exec_depend>
<exec_depend>kalman</exec_depend>
<exec_depend>ddynamic_reconfigure</exec_depend>
<exec_depend>costmap_2d</exec_depend>
<exec_depend>base_local_planner</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<depend>pluginlib</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<score_algorithm plugin="${prefix}/plugins.xml"/>
</export>
</package>

68
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp Normal file
View File

@ -0,0 +1,68 @@
#include <mkt_algorithm/go_straight.h>
#include <nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
nh_ = ros::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
costmap_ros_ = costmap_ros;
steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
}
void GoStraight::reset()
{
time_last_cmd_ = ros::Time::now();
}
bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction)
{
goal_ = goal;
const double rb_direction_x = goal_.x - pose.x;
const double rb_direction_y = goal_.y - pose.y;
const double rb_angle = atan2(rb_direction_y, rb_direction_x);
double rb_dir = cos(rb_angle- pose.theta);
x_direction = y_direction = theta_direction = rb_dir;
return true;
}
nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
{
nav_2d_msgs::Twist2DStamped result;
result.header.stamp = ros::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;
double odom_alpha = steer_to_baselink_pose.theta;
if(fabs(odom_alpha) > 0.03) return result;
double delta_x = goal_.x - pose.x;
double delta_y = goal_.y - pose.y;
double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2));
double d_reduce = std::min(std::max(fabs(velocity.x), 0.6), 0.9);
double v_min = 0.03 * velocity.x / (fabs(velocity.x) + 1e-9);
// Van toc robot
double vr = d_rg > d_reduce? velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min;
result.velocity.x = fabs(cos(fabs(0.0 - odom_alpha))) * vr / cos(std::min(M_PI/3, fabs(odom_alpha)));
// ROS_INFO("d_rg %f d_reduce %f V = %f %f", d_rg, d_reduce,velocity.x, vr);
return result;
}
}; // namespace mkt_algorithm
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::GoStraight, score_algorithm::ScoreAlgorithm)

641
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp Normal file
View File

@ -0,0 +1,641 @@
#include <mkt_algorithm/bicycle.h>
#include <pluginlib/class_list_macros.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
nh_priv_ = ros::NodeHandle(nh, name);
nh_ = ros::NodeHandle("~");
name_ = name;
tf_ = tf;
costmap_ros_ = costmap_ros;
last_actuator_update_ = ros::Time(0);
nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
nh_priv_.param("index_samples", index_samples_, 0);
nh_priv_.param("filter", is_filter_, false);
steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1);
steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.);
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1);
acc_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1);
decel_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1);
controller_frequency_param_name_ = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
if(controller_frequency_param_name_ <= 0.0)
{
ROS_WARN("controller_frequency is not setting lower 0.0 so will set to 1.0");
controller_frequency_param_name_ = 1.0;
}
reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
cmd_raw_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 1);
x_direction_ = y_direction_= theta_direction_ = 0;
// kalman
int n = 3; // Number of states
int m = 1; // Number of measurements
double dt = 1.0/controller_frequency_param_name_; // Time step
// ROS_INFO("controller_frequency_param_name %f", controller_frequency_param_name);
Eigen::MatrixXd A(n, n); // System dynamics matrix
Eigen::MatrixXd C(m, n); // Output matrix
Eigen::MatrixXd Q(n, n); // Process noise covariance
Eigen::MatrixXd R(m, m); // Measurement noise covariance
Eigen::MatrixXd P(n, n); // Estimate error covariance
// Discrete LTI projectile motion, measuring position only
A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
C << 1, 0, 0;
// Reasonable covariance matrices
double q = 0.1;
double r = 0.6;
double p = 0.2;
Q << q, q, 0.0, q, q, 0.0, 0.0, 0.0, 0.0;
R << r;
P << p, p, p, p, 10.0, 1, p, 1, 10.0;
// Construct the filter
kf_ = boost::make_shared<KalmanFilter>(dt,A, C, Q, R, P);
// Best guess of initial states
Eigen::VectorXd x0(n);
x0 << 0.0, 0.0, 0.0;
kf_->init(ros::Time::now().toSec(), x0);
}
void Bicycle::reset()
{
reached_intermediate_goals_.clear();
start_index_saved_vt_.clear();
start_index_saved_ = 0;
sub_goal_index_saved_ = 0;
sub_start_index_saved_ = 0;
x_direction_ = y_direction_= theta_direction_ = 0;
follow_step_path_ = false;
last_actuator_update_ = ros::Time(0);
}
bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction)
{
nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
nh_priv_.param("index_samples", index_samples_, 0);
nh_priv_.param("distance_rg", distance_rg_, 0.1);
nh_priv_.param("k_ld", k_ld_, 1.0);
nh_priv_.param("k_d_reduce", k_d_reduce_, 1.0);
nh_priv_.param("ld_min", ld_min_, 0.2);
nh_priv_.param("ld_max", ld_max_, 1.1);
nh_priv_.param("follow_step_path", follow_step_path_, false);
// ROS_INFO("%f %f %f %d", xy_local_goal_tolerance_, xy_local_goal_tolerance_offset_, angle_threshold_, index_samples_);
frame_id_path_ = global_plan.header.frame_id;
goal_ = goal;
global_plan_ = global_plan;
double rb_direction;
if(!getGoalPose(pose, velocity,global_plan, sub_goal_, rb_direction))
{
ROS_ERROR("getGoalPose Failed");
return false;
}
x_direction_= fabs(rb_direction)/rb_direction;
x_direction = x_direction_;
y_direction = y_direction_ = 0;
theta_direction = theta_direction_;
return true;
}
nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
{
nav_2d_msgs::Twist2DStamped result;
result.header.stamp = ros::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;
double actual_phi_steer = steer_to_baselink_pose.theta;
const double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
const double delta_x = sub_goal_.x - pose.x;
const double delta_y = sub_goal_.y - pose.y;
const double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2));
// Van toc robot
double d_reduce = std::min(std::max(fabs(velocity.x) * k_d_reduce_, 0.6), 3.0);
double v_min = 0.05 * velocity.x / (fabs(velocity.x) + 1e-9);
double vr = d_rg > d_reduce? velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min;
// Góc lái của bánh lái là: α(t)
double alpha = atan2(delta_y,delta_x) - pose.theta;
double ld = std::min(std::max(fabs(velocity.x)*k_ld_, ld_min_), ld_max_);
// double ld = std::min(std::max(d_rg*k_ld_, ld_min_), ld_max_);
double phi_steer = atan2(2 * L * sin(alpha), ld);
if(d_rg <= distance_rg_ && global_plan_.poses.size() > 1)
{
const double d_x = goal_.x - sub_goal_.x;
const double d_y = goal_.y - sub_goal_.y;
const double dd = sqrt(d_x * d_x + d_y * d_y);
const double d_theta = goal_.theta - sub_goal_.theta;
if(dd < 1e-5)
{
geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
phi_steer = atan2(2 * L * sin(alpha), 1.0);
}
}
double alpha_reduce = std::min(std::max(fabs(vr), 0.6), 1.0);
double cos_actual_phi_steer = cos( std::min(M_PI/2.3, fabs(actual_phi_steer)));
// limit W
double result_W_max = 0.25;
// double result_W_min = 0.13;
double result_W = vr * fabs(cos(alpha)) * tan(actual_phi_steer) / L;
double V_s = std::max(alpha_reduce, fabs(cos(alpha))) * vr / cos_actual_phi_steer;
std::stringstream ss;
if(fabs(result_W) > 0.02)
{
double d_w = fabs(result_W) - fabs(velocity.theta);
double a = fabs(velocity.theta) > fabs(result_W) ? fabs(acc_lim_theta_/controller_frequency_param_name_) : fabs(decel_lim_theta_/controller_frequency_param_name_);
// ss << d_w << " " << a << " ";
double result;
if(fabs(d_w) >= a)
result = fabs(velocity.theta) + d_w / fabs(d_w + 1e-9) * a;
else if (fabs(d_w) < fabs(a))
result = fabs(velocity.theta);
// ss << result_W << " " << velocity.theta << " ";
result_W = result_W / (fabs(result_W) + 1e-9) * result;
// ss << result_W << " ";
// ROS_INFO_STREAM(ss.str());
double d_v = fabs(result_W * L / tan(actual_phi_steer)) - fabs(vr);
if(fabs(d_v) >= a)
result = velocity.x / (fabs(velocity.x) + 1e-9)* (fabs(vr) + d_v / fabs(d_v + 1e-9) * a);
else if (fabs(d_v) < fabs(a))
result = velocity.x / (fabs(velocity.x) + 1e-9) * fabs(vr);
V_s = std::max(alpha_reduce, fabs(cos(alpha)))*result/ cos_actual_phi_steer;
}
if(fabs(result_W) > result_W_max)
V_s = ( result_W / (fabs(result_W) + 1e-9) * result_W_max * L / tan(actual_phi_steer)) / cos_actual_phi_steer;
if(is_filter_)
{
geometry_msgs::Twist msg;
msg.linear.x = result.velocity.x;
msg.angular.z = phi_steer;
cmd_raw_pub_.publish(msg);
Eigen::VectorXd y(1);
y << phi_steer;
ros::Time current_time = ros::Time::now();
double dt = ros::Duration(current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
Eigen::MatrixXd A(3, 3); // System dynamics matrix
A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
kf_->update(y, dt, A);
phi_steer = kf_->state().transpose()[0];
}
theta_direction_ = phi_steer !=0 ? phi_steer/(fabs(phi_steer) + 1e-9) * velocity.x/((fabs(velocity.x) + 1e-9)) : 0;
result.velocity.theta = phi_steer;
result.velocity.x = fabs(cos(phi_steer - actual_phi_steer)) * V_s;
return result;
}
unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
const double start_direction_x = plan[start_index + 1].x - plan[start_index].x;
const double start_direction_y = plan[start_index + 1].y - plan[start_index].y;
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) { // make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2));
// double angle_threshold = 0.0;
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
{
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2));
S += dd;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
if(fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance)
{
break;
}
goal_index = i;
}
}
}
return goal_index;
}
bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity,
const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal,
double &target_direction)
{
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
{
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
// Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = global_plan.poses;
std::vector<unsigned int> index_s;
if(!findSubGoalIndex(plan, index_s))
{
ROS_ERROR_NAMED("PathProgressCritic", "Can not find SubGoal Index");
return false;
}
// else
// {
// std::stringstream ss;
// for(auto i : index_s) ss << i << " ";
// ROS_INFO_STREAM(ss.str());
// }
unsigned int sub_goal_index = sub_goal_index_saved_;
unsigned int sub_start_index = sub_start_index_saved_;
if(index_s.empty() || index_s.size() < 1)
{
sub_goal_index = (unsigned int)plan.size();
sub_goal_index_saved_ = sub_goal_index;
}
else
{
if(index_s.front() > sub_goal_index_saved_)
{
sub_goal_index = (unsigned int)(index_s.front()) + 1;
sub_goal_index_saved_ = sub_goal_index;
}
if(index_s.size() > 1)
{
for(int i = 1; i < index_s.size(); ++i)
{
// double tolerance = line_generator->calculateTolerance(robot_pose, plan[index_s[i-1]]);
double distance = fabs(nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]]));
// double dx = plan[index_s[i-1]].x - robot_pose.x;
// double dy = plan[index_s[i-1]].y - robot_pose.y;
// if(std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_goal_tolerance_ + 0.15)
if (distance < xy_local_goal_tolerance_offset_)
{
if(index_s[i] > sub_goal_index_saved_)
{
if(i < index_s.size()-1)
{
sub_goal_index = index_s[i] + 1;
}
else
{
sub_goal_index = (unsigned int)plan.size();
}
if(follow_step_path_)
{
sub_start_index = index_s[i-1];
sub_start_index_saved_ = sub_start_index;
}
ROS_INFO("Tang nac path tu %d den %d boi khoang cach", sub_goal_index_saved_, sub_goal_index);
// ROS_WARN("i %d follow_step_path_ = %x start = %d sub_goal = %d size = %d", i, follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
sub_goal_index_saved_ = sub_goal_index;
break;
}
}
}
}
else
{
double distance = fabs(nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose));
if(distance <= xy_local_goal_tolerance_ )
{
sub_goal_index = (unsigned int)plan.size();
sub_goal_index_saved_ = sub_goal_index;
}
}
}
// find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
// Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi
double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
bool started_path = false;
// for (unsigned int i = 0; i < plan.size(); i++)
// ROS_INFO_THROTTLE(1.0, "follow_step_path_ = %x start = %d sub_goal = %d size = %d", follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
for (unsigned int i = sub_start_index; i < sub_goal_index; i++)
{
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
// && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// {
// Still on the costmap. Continue.
double distance = fabs(nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
if (distance_to_start > distance)
{
start_index = i;
distance_to_start = distance;
started_path = true;
}
// else
// {
// break;
// }
// else if (started_path)
// {
// // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// // even closer to the robot, but then we would skip over parts of the plan.
// break;
// }
// else, we have not yet found a point on the costmap, so we just continue
}
// Nếu khồng tìm được điểm gần với robot nhất thì trả về false
if (!started_path)
{
ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
// find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
if(start_index < 1) start_index == 1;
// unsigned int last_valid_index = plan.size() - 1;
unsigned int last_valid_index = sub_goal_index - 1;
// for (unsigned int i = start_index + 1; i < sub_goal_index; i++)
// {
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// {
// // Still on the costmap. Continue.
// last_valid_index = i;
// }
// else
// {
// // Off the costmap after being on the costmap.
// break;
// }
// }
// find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// i.e. is within angle_threshold_ of the starting direction.
// Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
unsigned int goal_index = start_index;
ros::Rate r(30);
while (goal_index < last_valid_index && ros::ok())
{
// Tìm kiếm vị trí điểm mục tiêu phù hợp
double S;
if(velocity.x > 0) S = std::min(4.0, std::max(0.9, fabs(velocity.x)*k_d_reduce_));
// else if(velocity.x < 0) S = 0.5;
else if(velocity.x < 0) S = std::min(0.7, std::max(0.5, fabs(velocity.x)*k_d_reduce_));
else S = 0.5;
goal_index = getGoalIndex(robot_pose, plan, S, start_index, last_valid_index);
// check if goal already reached
bool goal_already_reached = false;
geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = ros::Time::now();
poseArrayMsg.header.frame_id = frame_id_path_;
// std::stringstream ss;
for (auto &reached_intermediate_goal : reached_intermediate_goals_)
{
double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
// ss << distance << " ";
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal);
poseArrayMsg.poses.push_back(pose);
}
reached_intermediate_goals_pub_.publish(poseArrayMsg);
// if(!reached_intermediate_goals_.empty())
// {
// ROS_INFO_STREAM(ss.str());
// }
for (auto &reached_intermediate_goal : reached_intermediate_goals_)
{
double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
if (distance < xy_local_goal_tolerance_)
{
goal_already_reached = true;
// choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// (start_index is now > goal_index)
for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
{
distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
if ( distance >= xy_local_goal_tolerance_offset_)
{
break;
}
}
// start_index_saved_ = start_index;
break;
}
}
//try hiep
unsigned num = 0;
start_index_saved_vt_.insert(start_index_saved_vt_.begin(),start_index);
if(start_index_saved_vt_.size() > index_samples_) start_index_saved_vt_.pop_back();
std::stringstream ss;
for(int i = 0; i < start_index_saved_vt_.size() - 1; i++)
{
double edge = 0;
if(start_index_saved_vt_[i] != start_index_saved_vt_[i+1])
edge = journey(plan, start_index_saved_vt_[i], start_index_saved_vt_[i+1]);
if(edge < xy_local_goal_tolerance_) num++;
ss << start_index_saved_vt_[i] << " ";
}
// ss << start_index << " ";
if(num < index_samples_ - 1 && !start_index_saved_vt_.empty())
{
auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end()));
start_index_saved_ = start_index_saved_ >= max_iter? start_index_saved_ : max_iter;
double S1 = journey(plan, start_index, plan.size()-1 );
double S2 = journey(plan, start_index_saved_, plan.size()-1 );
// ROS_INFO_STREAM(ss.str());
if(S2 - S1 < -fabs(xy_local_goal_tolerance_))
{
start_index = start_index_saved_;
}
else start_index_saved_ = start_index;
// ROS_INFO(" %d %d %f %d %f %f", max_iter, start_index, S1, start_index_saved_, S2, S2 - S1);
}
else start_index_saved_ = start_index;
if(start_index >= goal_index) goal_index = getGoalIndex(robot_pose, plan, S,start_index, last_valid_index);
if (!goal_already_reached)
{
// new goal not in reached_intermediate_goals_
double distance = fabs(nav_2d_utils::poseDistance(plan[goal_index], plan[start_index]));
if (distance < xy_local_goal_tolerance_)
{
// ROS_INFO("goal_index %d distance %f", goal_index, distance);
// the robot has currently reached the goal
reached_intermediate_goals_.push_back(plan[goal_index]);
ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
}
else
{
// we've found a new goal!
break;
}
}
r.sleep();
ros::spinOnce();
}
if (start_index > goal_index)
start_index = goal_index;
ROS_ASSERT(goal_index <= last_valid_index);
if((unsigned int)plan.size() == 2 && !plan.empty())
{
start_index = 0;
goal_index = 1;
}
// Tham chiếu lại đối số đầu vao
sub_goal = plan[goal_index];
if(goal_index > 0) rev_sub_goal_ = plan[goal_index - 1];
else rev_sub_goal_ = plan[start_index];
target_direction = x_direction_;
if(goal_index > 0 && (unsigned int)plan.size() > 1)
{
if(goal_index < (unsigned int)(plan.size() -1))
{
const geometry_msgs::Pose2D p1 = plan[start_index];
int index;
for(index = start_index+1; index < goal_index; index++)
{
geometry_msgs::Pose2D pose = plan[index];
if(nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_) break;
}
const geometry_msgs::Pose2D p2 = plan[index];
if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
{
const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
const double dir_path = cos(fabs(path_angle - p2.theta));
if(fabs(dir_path) > 1e-9)
target_direction = dir_path;
// ROS_INFO("%f %f %f %f", p1.x ,p1.y, p2.x ,p2.y);
// ROS_INFO("%d %0.2f %0.2f %0.2f", goal_index, path_angle, p2.theta, target_direction);
}
}
else
{
const double rb_direction_x = sub_goal.x - robot_pose.x;
const double rb_direction_y = sub_goal.y - robot_pose.y;
const double rb_angle = atan2(rb_direction_y, rb_direction_x);
double dir_rg = cos(rb_angle - robot_pose.theta);
const double angle_threshold = 0.6;
if(fabs(dir_rg) > angle_threshold)
target_direction = dir_rg;
}
}
// Publish start_index
geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, ros::Time::now());
closet_robot_goal_pub_.publish(pose_st);
// Publish goal_index
geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, ros::Time::now());
current_goal_pub_.publish(pose_g);
return true;
}
bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
{
if (plan.empty())
{
return false;
}
double x_direction_saved = 0.0;
geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = ros::Time::now();
poseArrayMsg.header.frame_id = frame_id_path_;
for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
{
geometry_msgs::Pose2D p1 = plan[i];
geometry_msgs::Pose2D p2 = plan[i+1];
// geometry_msgs::Pose2D p3 = plan[i+2];
double angle1;
if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
else continue;
// const double angle2 = atan2(p3.y - p2.y, p3.x - p2.x);
const double target_direction = cos(fabs(angle1 - p2.theta));
if(target_direction * x_direction_saved < 0.0)
{
index_s.push_back(i);
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(plan[i]);
poseArrayMsg.poses.push_back(pose);
}
x_direction_saved = target_direction;
}
if(index_s.empty())
index_s.push_back((unsigned int)plan.size() - 1);
else
{
if(index_s.back() < (unsigned int)plan.size() - 1)
index_s.push_back((unsigned int)plan.size() - 1);
}
reached_intermediate_goals_pub_.publish(poseArrayMsg);
return true;
}
double Bicycle::journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
{
double S = 0;
if(last_valid_index - start_index > 1)
{
for(int i= start_index; i < last_valid_index; i++)
{
S += nav_2d_utils::poseDistance(plan[i], plan[i+1]);
}
}
return S;
}
} // namespace mkt_algorithm
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::Bicycle, score_algorithm::ScoreAlgorithm)

78
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp Normal file
View File

@ -0,0 +1,78 @@
#include <mkt_algorithm/rotate_to_goal.h>
#include <nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
nh_ = ros::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
costmap_ros_ = costmap_ros;
steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
nh_.param("velocity_rotate_min", velocity_rotate_min_, 0.1);
nh_.param("velocity_rotate_max", velocity_rotate_max_, 0.6);
nh_.param("angle_pass_rotate", angle_threshold_, 0.02);
}
void RotateToGoal::reset()
{
time_last_cmd_ = ros::Time::now();
}
bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction)
{
goal_ = goal;
double angle = angles::shortest_angular_distance(pose.theta, goal_.theta);
if(angle > 0) current_direction_ = 1;
else if(angle < 0) current_direction_ = -1;
x_direction = y_direction = theta_direction = current_direction_;
return true;
}
nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity)
{
nav_2d_msgs::Twist2DStamped result;
result.header.stamp = ros::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;
double actual_phi_steer = steer_to_baselink_pose.theta;
double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
// ros::Time t = ros::Time::now();
// double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec();
// time_last_cmd_ = ros::Time::now();
double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta);
double angle_reduce = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_);
double w_min = velocity.theta / (fabs(velocity.theta) + 1e-9) * velocity_rotate_min_;
double w_r = fabs(angle_rg) > angle_reduce? velocity.theta : (velocity.theta - w_min)*sin((fabs(angle_rg) / angle_reduce) * (M_PI/2)) + w_min;
int direct_rotate = (angle_rg/fabs(angle_rg));
double target_phi_steer = direct_rotate * M_PI/2;
if( fabs(target_phi_steer - actual_phi_steer) <= angle_threshold_)
{
double target_v = fabs(w_r * L);
result.velocity.x = std::min(velocity_rotate_max_ * L, fabs(target_v));
}
else result.velocity.x = 0.0;
result.velocity.theta = target_phi_steer;
return result;
}
}; // namespace mkt_algorithm
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::RotateToGoal, score_algorithm::ScoreAlgorithm)

243
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp Normal file
View File

@ -0,0 +1,243 @@
#include <mkt_algorithm/diff/diff_go_straight.h>
// other
// #include <pluginlib/class_list_macros.h>
#include <boost/dll/alias.hpp>
void mkt_algorithm::diff::GoStraight::initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
if (!initialized_)
{
nh_ = ros::NodeHandle("~/");
nh_priv_ = ros::NodeHandle("~/" + name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_ros_ = costmap_ros;
this->getParams();
// this->initDynamicReconfigure(nh_priv_);
nh_.param("publish_topic", enable_publish_, false);
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
std::vector<geometry_msgs::Point> footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
double max_length = 0.0;
for (size_t i = 0; i < footprint.size(); ++i)
{
const geometry_msgs::Point &p1 = footprint[i];
const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
if (len > max_length)
{
max_length = len;
}
if (len < min_length)
{
min_length = len;
}
}
this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
}
// kalman
last_actuator_update_ = ros::Time::now();
n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
m_ = 2; // measurements: x, y, theta
double dt = control_duration_;
// Khởi tạo ma trận
A = Eigen::MatrixXd::Identity(n_, n_);
C = Eigen::MatrixXd::Zero(m_, n_);
Q = Eigen::MatrixXd::Zero(n_, n_);
R = Eigen::MatrixXd::Identity(m_, m_);
P = Eigen::MatrixXd::Identity(n_, n_);
for (int i = 0; i < n_; i += 3)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
}
C(0, 0) = 1;
C(1, 3) = 1;
Q(2, 2) = 0.1;
Q(5, 5) = 0.6;
R(0, 0) = 0.1;
R(1, 1) = 0.2;
P(3, 3) = 0.4;
P(4, 4) = 0.4;
P(5, 5) = 0.4;
kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
Eigen::VectorXd x0(n_);
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(ros::Time::now().toSec(), x0);
x_direction_ = y_direction_ = theta_direction_ = 0;
initialized_ = true;
ROS_INFO("GoStraight Initialized successfully");
}
}
mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
{
mkt_msgs::Trajectory2D result;
result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
if (!traj_)
return result;
if (compute_plan_.poses.size() < 2)
{
ROS_WARN_THROTTLE(2.0, "Local compute plan is available");
return result;
}
ros::Time current_time = ros::Time::now();
double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
nav_2d_msgs::Twist2D drive_cmd;
double sign_x = x_direction_;
nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
ros::Rate r(50);
while (ros::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
// ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
r.sleep();
}
drive_cmd.x = sqrt(twist.x * twist.x);
nav_2d_msgs::Path2D transformed_plan = transform_plan_;
if (transformed_plan.poses.empty())
{
ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
return result;
}
if (enable_publish_)
transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
// Dynamically adjust look ahead distance based on the speed
double lookahead_dist = this->getLookAheadDistance(twist);
// Return false if the transformed global plan is empty
geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
lookahead_dist += fabs(front.y);
// Get lookahead point and publish for visualization
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
if (enable_publish_)
carrot_pub_.publish(this->createCarrotMsg(carrot_pose));
// Carrot distance squared
const double carrot_dist2 =
(carrot_pose.pose.x * carrot_pose.pose.x) +
(carrot_pose.pose.y * carrot_pose.pose.y);
// Find curvature of circle (k = 1 / R)
double curvature = 0.0;
if (carrot_dist2 > 2e-2)
{
curvature = 2.0 * carrot_pose.pose.y / carrot_dist2;
}
// Constrain linear velocity
this->applyConstraints(
0.0, lookahead_dist, curvature, twist,
this->costAtPose(pose.pose.x, pose.pose.y),
drive_cmd.x, sign_x);
if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_)
{
nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
drive_cmd.theta = atan2(dy, dx);
//[-π/2, π/2]
if (drive_cmd.theta > M_PI / 2)
drive_cmd.theta -= M_PI;
else if (drive_cmd.theta < -M_PI / 2)
drive_cmd.theta += M_PI;
drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_);
}
else
drive_cmd.theta = 0.0;
// populate and return message
if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9)
drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)));
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
const double vel_x_reduce = std::min(fabs(v_max), 0.3);
double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
journey_plan += max_path_distance_;
drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min;
// drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min;
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// Cập nhật lại A nếu dt thay đổi
for (int i = 0; i < n_; ++i)
for (int j = 0; j < n_; ++j)
A(i, j) = (i == j ? 1.0 : 0.0);
for (int i = 0; i < n_; i += 3)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
}
kf_->update(y, dt, A);
if (drive_cmd.x != v_max)
{
double min = drive_cmd.x < v_max ? drive_cmd.x : v_max;
double max = drive_cmd.x > v_max ? drive_cmd.x : v_max;
drive_cmd.x = std::clamp(kf_->state()[0], min, max);
}
else
{
drive_cmd.x = std::clamp(kf_->state()[0], v_max, v_max);
}
drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
drive_cmd.theta = kf_->state()[3];
result.velocity = drive_cmd;
return result;
}
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm)

1587
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

131
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp Normal file
View File

@ -0,0 +1,131 @@
#include <mkt_algorithm/diff/diff_rotate_to_goal.h>
// other
#include <nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
void mkt_algorithm::diff::RotateToGoal::initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
if (!initialized_)
{
nh_ = ros::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_ros_ = costmap_ros;
this->getParams();
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
ROS_INFO("RotateToGoal Initialized successfully");
}
}
void mkt_algorithm::diff::RotateToGoal::getParams()
{
robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
nh_priv_.param<int>("index_samples", index_samples_, 0);
nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);
nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());
nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
nh_priv_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
// Regulated linear velocity scaling
nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
// Inflation cost scaling (Limit velocity by proximity to obstacles)
nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
if (inflation_cost_scaling_factor_ <= 0.0)
{
ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
"it should be >0. Disabling cost regulated linear velocity scaling.");
use_cost_regulated_linear_velocity_scaling_ = false;
}
double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
if (traj_)
{
traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
}
}
void mkt_algorithm::diff::RotateToGoal::reset()
{
this->clear();
}
bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity,
const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction)
{
if (!initialized_)
{
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
this->getParams();
global_plan_ = global_plan;
goal_ = goal;
double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
x_direction = y_direction = theta_direction = sign(angle);
;
return true;
}
mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
{
mkt_msgs::Trajectory2D result;
nav_2d_msgs::Twist2D drive_cmd;
result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
if (!traj_)
return result;
const double dt = control_duration_;
double vel_yaw = velocity.theta;
double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
rotateToHeading(ang_diff, velocity, drive_cmd);
drive_cmd.x = 0.0;
drive_cmd.y = 0.0;
result.velocity = drive_cmd;
return result;
}
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm)

56
src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,56 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Cho phép các project khác include được header của mkt_msgs
set(mkt_msgs_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo INTERFACE library (header-only)
add_library(mkt_msgs INTERFACE)
target_link_libraries(mkt_msgs INTERFACE nav_2d_msgs geometry_msgs)
# Set include directories
target_include_directories(mkt_msgs
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/mkt_msgs
DESTINATION include
FILES_MATCHING PATTERN "*.h")
install(TARGETS mkt_msgs
EXPORT mkt_msgs-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT mkt_msgs-targets
FILE mkt_msgs-targets.cmake
DESTINATION lib/cmake/mkt_msgs)
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

66
src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h Normal file
View File

@ -0,0 +1,66 @@
// Generated by gencpp from file mkt_msgs/Trajectory2D.msg
// DO NOT EDIT!
#ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H
#define MKT_MSGS_MESSAGE_TRAJECTORY2D_H
#include <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Twist2D.h>
#include <geometry_msgs/Pose2D.h>
namespace mkt_msgs
{
template <class ContainerAllocator>
struct Trajectory2D_
{
typedef Trajectory2D_<ContainerAllocator> Type;
Trajectory2D_()
: velocity(), poses(), time_offsets()
{
}
Trajectory2D_(const ContainerAllocator &_alloc)
: velocity(_alloc), poses(_alloc), time_offsets(_alloc)
{
(void)_alloc;
}
typedef ::nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef std::vector<geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<geometry_msgs::Pose2D>> _poses_type;
_poses_type poses;
typedef std::vector<robot::Duration, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot::Duration>> _time_offsets_type;
_time_offsets_type time_offsets;
typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_<ContainerAllocator>> Ptr;
typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_<ContainerAllocator> const> ConstPtr;
}; // struct Trajectory2D_
typedef ::mkt_msgs::Trajectory2D_<std::allocator<void>> Trajectory2D;
typedef boost::shared_ptr<::mkt_msgs::Trajectory2D> Trajectory2DPtr;
typedef boost::shared_ptr<::mkt_msgs::Trajectory2D const> Trajectory2DConstPtr;
template <typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::mkt_msgs::Trajectory2D_<ContainerAllocator1> &lhs, const ::mkt_msgs::Trajectory2D_<ContainerAllocator2> &rhs)
{
return lhs.velocity == rhs.velocity &&
lhs.poses == rhs.poses &&
lhs.time_offsets == rhs.time_offsets;
}
template <typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::mkt_msgs::Trajectory2D_<ContainerAllocator1> &lhs, const ::mkt_msgs::Trajectory2D_<ContainerAllocator2> &rhs)
{
return !(lhs == rhs);
}
} // namespace mkt_msgs
#endif // MKT_MSGS_MESSAGE_TRAJECTORY2D_H

61
src/Libraries/nav_2d_msgs/CMakeLists.txt Executable file
View File

@ -0,0 +1,61 @@
cmake_minimum_required(VERSION 3.10)
project(nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# Tìm tất cả header files
file(GLOB_RECURSE HEADERS "include/nav_2d_msgs/*.h")
# Tạo INTERFACE library (header-only)
add_library(nav_2d_msgs INTERFACE)
# Set include directories
target_include_directories(nav_2d_msgs
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Link dependencies (header-only, chỉ cần include paths)
# Các dependencies này cũng là header-only từ common_msgs
target_link_libraries(nav_2d_msgs
INTERFACE
std_msgs
geometry_msgs
)
# Cài đặt header files
install(DIRECTORY include/nav_2d_msgs
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt target
install(TARGETS nav_2d_msgs
EXPORT nav_2d_msgs-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
# Bây giờ có thể export dependencies vì std_msgs và geometry_msgs đã được export
install(EXPORT nav_2d_msgs-targets
FILE nav_2d_msgs-targets.cmake
NAMESPACE nav_2d_msgs::
DESTINATION lib/cmake/nav_2d_msgs)
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "Dependencies: std_msgs, geometry_msgs")
message(STATUS "=================================")

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h Normal file
View File

@ -0,0 +1,74 @@
// Generated by gencpp from file nav_2d_msgs/ComplexPolygon2D.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
#define NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H
#include <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Polygon2D.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct ComplexPolygon2D_
{
typedef ComplexPolygon2D_<ContainerAllocator> Type;
ComplexPolygon2D_()
: outer()
, inner() {
}
ComplexPolygon2D_(const ContainerAllocator& _alloc)
: outer(_alloc)
, inner(_alloc) {
(void)_alloc;
}
typedef ::nav_2d_msgs::Polygon2D_<ContainerAllocator> _outer_type;
_outer_type outer;
typedef std::vector< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> >> _inner_type;
_inner_type inner;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct ComplexPolygon2D_
typedef ::nav_2d_msgs::ComplexPolygon2D_<std::allocator<void> > ComplexPolygon2D;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return lhs.outer == rhs.outer &&
lhs.inner == rhs.inner;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H

94
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h Normal file
View File

@ -0,0 +1,94 @@
// Generated by gencpp from file nav_2d_msgs/NavGridInfo.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
#define NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H
#include <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridInfo_
{
typedef NavGridInfo_<ContainerAllocator> Type;
NavGridInfo_()
: width(0)
, height(0)
, resolution(0.0)
, frame_id()
, origin_x(0.0)
, origin_y(0.0) {
}
NavGridInfo_(const ContainerAllocator& _alloc)
: width(0)
, height(0)
, resolution(0.0)
, frame_id(_alloc)
, origin_x(0.0)
, origin_y(0.0) {
(void)_alloc;
}
typedef uint32_t _width_type;
_width_type width;
typedef uint32_t _height_type;
_height_type height;
typedef double _resolution_type;
_resolution_type resolution;
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _frame_id_type;
_frame_id_type frame_id;
typedef double _origin_x_type;
_origin_x_type origin_x;
typedef double _origin_y_type;
_origin_y_type origin_y;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridInfo_
typedef ::nav_2d_msgs::NavGridInfo_<std::allocator<void> > NavGridInfo;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo > NavGridInfoPtr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
{
return lhs.width == rhs.width &&
lhs.height == rhs.height &&
lhs.resolution == rhs.resolution &&
lhs.frame_id == rhs.frame_id &&
lhs.origin_x == rhs.origin_x &&
lhs.origin_y == rhs.origin_y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h Normal file
View File

@ -0,0 +1,80 @@
// Generated by gencpp from file nav_2d_msgs/NavGridOfChars.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H
#include <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/NavGridInfo.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfChars_
{
typedef NavGridOfChars_<ContainerAllocator> Type;
NavGridOfChars_()
: stamp()
, info()
, data() {
}
NavGridOfChars_(const ContainerAllocator& _alloc)
: stamp()
, info(_alloc)
, data(_alloc) {
(void)_alloc;
}
typedef robot::Time _stamp_type;
_stamp_type stamp;
typedef ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfChars_
typedef ::nav_2d_msgs::NavGridOfChars_<std::allocator<void> > NavGridOfChars;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h Normal file
View File

@ -0,0 +1,80 @@
// Generated by gencpp from file nav_2d_msgs/NavGridOfCharsUpdate.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H
#include <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/UIntBounds.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfCharsUpdate_
{
typedef NavGridOfCharsUpdate_<ContainerAllocator> Type;
NavGridOfCharsUpdate_()
: stamp()
, bounds()
, data() {
}
NavGridOfCharsUpdate_(const ContainerAllocator& _alloc)
: stamp()
, bounds(_alloc)
, data(_alloc) {
(void)_alloc;
}
typedef robot::Time _stamp_type;
_stamp_type stamp;
typedef ::nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfCharsUpdate_
typedef ::nav_2d_msgs::NavGridOfCharsUpdate_<std::allocator<void> > NavGridOfCharsUpdate;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h Normal file
View File

@ -0,0 +1,80 @@
// Generated by gencpp from file nav_2d_msgs/NavGridOfDoubles.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H
#include <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/NavGridInfo.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoubles_
{
typedef NavGridOfDoubles_<ContainerAllocator> Type;
NavGridOfDoubles_()
: stamp()
, info()
, data() {
}
NavGridOfDoubles_(const ContainerAllocator& _alloc)
: stamp()
, info(_alloc)
, data(_alloc) {
(void)_alloc;
}
typedef robot::Time _stamp_type;
_stamp_type stamp;
typedef ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoubles_
typedef ::nav_2d_msgs::NavGridOfDoubles_<std::allocator<void> > NavGridOfDoubles;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h Normal file
View File

@ -0,0 +1,80 @@
// Generated by gencpp from file nav_2d_msgs/NavGridOfDoublesUpdate.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H
#include <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/UIntBounds.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoublesUpdate_
{
typedef NavGridOfDoublesUpdate_<ContainerAllocator> Type;
NavGridOfDoublesUpdate_()
: stamp()
, bounds()
, data() {
}
NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc)
: stamp()
, bounds(_alloc)
, data(_alloc) {
(void)_alloc;
}
typedef robot::Time _stamp_type;
_stamp_type stamp;
typedef ::nav_2d_msgs::UIntBounds_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoublesUpdate_
typedef ::nav_2d_msgs::NavGridOfDoublesUpdate_<std::allocator<void> > NavGridOfDoublesUpdate;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h Normal file
View File

@ -0,0 +1,74 @@
// Generated by gencpp from file nav_2d_msgs/Path2D.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_PATH2D_H
#define NAV_2D_MSGS_MESSAGE_PATH2D_H
#include <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/Pose2DStamped.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Path2D_
{
typedef Path2D_<ContainerAllocator> Type;
Path2D_()
: header()
, poses() {
}
Path2D_(const ContainerAllocator& _alloc)
: header()
, poses(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header _header_type;
_header_type header;
typedef std::vector< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;
_poses_type poses;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> const> ConstPtr;
}; // struct Path2D_
typedef ::nav_2d_msgs::Path2D_<std::allocator<void> > Path2D;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D > Path2DPtr;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D const> Path2DConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.poses == rhs.poses;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_PATH2D_H

72
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h Normal file
View File

@ -0,0 +1,72 @@
// Generated by gencpp from file nav_2d_msgs/Point2D.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_POINT2D_H
#define NAV_2D_MSGS_MESSAGE_POINT2D_H
#include <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Point2D_
{
typedef Point2D_<ContainerAllocator> Type;
Point2D_()
: x(0.0)
, y(0.0) {
}
Point2D_(const ContainerAllocator& _alloc)
: x(0.0)
, y(0.0) {
(void)_alloc;
}
typedef double _x_type;
_x_type x;
typedef double _y_type;
_y_type y;
typedef std::shared_ptr< ::nav_2d_msgs::Point2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Point2D_<ContainerAllocator> const> ConstPtr;
}; // struct Point2D_
typedef ::nav_2d_msgs::Point2D_<std::allocator<void> > Point2D;
typedef std::shared_ptr< ::nav_2d_msgs::Point2D > Point2DPtr;
typedef std::shared_ptr< ::nav_2d_msgs::Point2D const> Point2DConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POINT2D_H

67
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h Normal file
View File

@ -0,0 +1,67 @@
// Generated by gencpp from file nav_2d_msgs/Polygon2D.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2D_H
#define NAV_2D_MSGS_MESSAGE_POLYGON2D_H
#include <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Point2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2D_
{
typedef Polygon2D_<ContainerAllocator> Type;
Polygon2D_()
: points() {
}
Polygon2D_(const ContainerAllocator& _alloc)
: points(_alloc) {
(void)_alloc;
}
typedef std::vector< ::nav_2d_msgs::Point2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Point2D_<ContainerAllocator> >> _points_type;
_points_type points;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2D_
typedef ::nav_2d_msgs::Polygon2D_<std::allocator<void> > Polygon2D;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D > Polygon2DPtr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D const> Polygon2DConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return lhs.points == rhs.points;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2D_H

81
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h Normal file
View File

@ -0,0 +1,81 @@
// Generated by gencpp from file nav_2d_msgs/Polygon2DCollection.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
#define NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H
#include <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/ComplexPolygon2D.h>
#include <std_msgs/ColorRGBA.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DCollection_
{
typedef Polygon2DCollection_<ContainerAllocator> Type;
Polygon2DCollection_()
: header()
, polygons()
, colors() {
}
Polygon2DCollection_(const ContainerAllocator& _alloc)
: header()
, polygons(_alloc)
, colors(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header _header_type;
_header_type header;
typedef std::vector< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
_polygons_type polygons;
typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
_colors_type colors;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DCollection_
typedef ::nav_2d_msgs::Polygon2DCollection_<std::allocator<void> > Polygon2DCollection;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygons == rhs.polygons &&
lhs.colors == rhs.colors;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h Normal file
View File

@ -0,0 +1,74 @@
// Generated by gencpp from file nav_2d_msgs/Polygon2DStamped.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
#define NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H
#include <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DStamped_
{
typedef Polygon2DStamped_<ContainerAllocator> Type;
Polygon2DStamped_()
: header()
, polygon() {
}
Polygon2DStamped_(const ContainerAllocator& _alloc)
: header()
, polygon(_alloc) {
(void)_alloc;
}
typedef ::std_msgs::Header _header_type;
_header_type header;
typedef ::nav_2d_msgs::Polygon2D_<ContainerAllocator> _polygon_type;
_polygon_type polygon;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DStamped_
typedef ::nav_2d_msgs::Polygon2DStamped_<std::allocator<void> > Polygon2DStamped;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygon == rhs.polygon;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H

78
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h Normal file
View File

@ -0,0 +1,78 @@
// Generated by gencpp from file nav_2d_msgs/Pose2D32.msg
// DO NOT EDIT!
#ifndef NAV_2D_MSGS_MESSAGE_POSE2D32_H
#define NAV_2D_MSGS_MESSAGE_POSE2D32_H
#include <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Pose2D32_
{
typedef Pose2D32_<ContainerAllocator> Type;
Pose2D32_()
: x(0.0)
, y(0.0)
, theta(0.0) {
}
Pose2D32_(const ContainerAllocator& _alloc)
: x(0.0)
, y(0.0)
, theta(0.0) {
(void)_alloc;
}
typedef float _x_type;
_x_type x;
typedef float _y_type;
_y_type y;
typedef float _theta_type;
_theta_type theta;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_<ContainerAllocator> const> ConstPtr;
}; // struct Pose2D32_
typedef ::nav_2d_msgs::Pose2D32_<std::allocator<void> > Pose2D32;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 > Pose2D32Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POSE2D32_H

Some files were not shown because too many files have changed in this diff Show More