论文复现【Switching Sampling Space of Model Predictive Path-Integral Controller to Balance Efficiency and Safety in 4WIDS Vehicle Navigation】

zylyehuo2025-09-24 13:30

博客地址：https://www.cnblogs.com/zylyehuo/
Switching Sampling Space of Model Predictive Path-Integral Controller to Balance Efficiency and Safety in 4WIDS Vehicle Navigation
GitHub项目链接

部署环境

  • Ubuntu 20.04
  • ROS Noetic
  • numpy 1.20.3

第一步：安装 osqp，osqp-eigen

ubuntu20.04 osqp，osqp-eigen和测试记录

安装 osqp

bash 复制代码 
git clone https://gitee.com/juanmv/osqp.git

cd osqp

mkdir build

cd build

cmake ..

sudo make install

安装 osqp-eigen

bash 复制代码 
git clone https://gitee.com/juanmv/osqp-eigen.git

cd osqp-eigen

mkdir build

cd build

cmake ..

sudo make install

测试代码

bash 复制代码 
git clone https://gitee.com/juanmv/osqp_test.git

cd osqp_test

mkdir build

cd build

cmake ..

make

./osqp_test

第二步：下载项目源码

bash 复制代码 
git clone https://github.com/MizuhoAOKI/nullspace_mpc

第三步：自动下载依赖项

bash 复制代码 
cd nullspace_mpc

sudo make install_deps

第四步：初始化并更新 rosdep，下载 ROS 依赖项

ROS依赖包安装问题/XXXXX:Cannot locate rosdep definition for [XXX]

bash 复制代码 
cd nullspace_mpc

sudo rosdep init # 如果已经显示初始化，则跳过这条指令，直接执行下一条

rosdep update

rosdep install -y --from-paths src --ignore-src --rosdistro noetic

第五步：Python 安装 jsk_data ROS 包

bash 复制代码 
sudo apt-get update

sudo apt-get install ros-noetic-jsk-data

第六步：几个主要文件的修改

/home/yehuo/nullspace_mpc_ws/Makefile

注意 C++ 编译器的版本是否和自己电脑的一致

makefile 复制代码 
# Usage: make [command]
# --- Variables ---
SHELL:=/bin/bash
PROJ_NAME=nullspace_mpc
VERSION=0.1.1
USER_NAME=noetic

# Docker image and container naming
DOCKER_IMAGE_BASE = $(PROJ_NAME):$(VERSION)
CONTAINER_NAME_BASE = $(PROJ_NAME)-container

# Workspace and X11 forwarding settings
WORKSPACE = $(shell pwd)
XSOCK = /tmp/.X11-unix
XAUTH = /tmp/.docker.xauth

# Phony targets to prevent conflicts with file names
.PHONY: build

# Build CPU image
setup_docker_cpu:
	docker build \
		--build-arg BASE_IMAGE=ubuntu:20.04 \
		--build-arg ROS_PACKAGE=ros-noetic-desktop \
		-t $(DOCKER_IMAGE_BASE)-cpu \
		-f docker/Dockerfile_cpu .

# Build GPU image
setup_docker_gpu:
	docker build \
		-t $(DOCKER_IMAGE_BASE)-gpu \
		-f docker/Dockerfile_gpu .

# Launch or attach to the CPU container
run_docker_cpu:
	@CONTAINER="$(CONTAINER_NAME_BASE)-cpu"; \
	if [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "running" ]; then \
		echo "Attaching to running container: $$CONTAINER"; \
		$(MAKE) exec_docker_cpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "exited" ]; then \
		echo "Restarting and attaching to container: $$CONTAINER"; \
		docker start $$CONTAINER && $(MAKE) exec_docker_cpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "created" ]; then \
		echo "Starting and attaching to container: $$CONTAINER"; \
		docker start $$CONTAINER && $(MAKE) exec_docker_cpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "paused" ]; then \
		echo "Unpausing and attaching to container: $$CONTAINER"; \
		docker unpause $$CONTAINER && $(MAKE) exec_docker_cpu; \
	else \
		echo "Creating and running new container: $$CONTAINER"; \
		docker run -it --name $$CONTAINER \
			--network host \
			--privileged \
			--ipc host \
			--volume=$(WORKSPACE):/home/$(USER_NAME)/$(PROJ_NAME):rw \
			--volume=$(XSOCK):$(XSOCK):rw \
			--env="DISPLAY=$(DISPLAY)" \
			--env="QT_X11_NO_MITSHM=1" \
			$(DOCKER_IMAGE_BASE)-cpu \
			bash; \
	fi

# Launch or attach to the GPU container
run_docker_gpu:
	@CONTAINER="$(CONTAINER_NAME_BASE)-gpu"; \
	if [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "running" ]; then \
		echo "Attaching to running container: $$CONTAINER"; \
		$(MAKE) exec_docker_gpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "exited" ]; then \
		echo "Restarting and attaching to container: $$CONTAINER"; \
		docker start $$CONTAINER && $(MAKE) exec_docker_gpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "created" ]; then \
		echo "Starting and attaching to container: $$CONTAINER"; \
		docker start $$CONTAINER && $(MAKE) exec_docker_gpu; \
	elif [ "$$(docker inspect -f '{{.State.Status}}' $$CONTAINER 2>/dev/null)" = "paused" ]; then \
		echo "Unpausing and attaching to container: $$CONTAINER"; \
		docker unpause $$CONTAINER && $(MAKE) exec_docker_gpu; \
	else \
		echo "Creating and running new container: $$CONTAINER"; \
		[ -e "$(XAUTH)" ] || install -m 600 /dev/null "$(XAUTH)"; \
		chmod 644 "$(XAUTH)" || true; \
		xauth nlist "$(DISPLAY)" | sed -e 's/^..../ffff/' | xauth -f "$(XAUTH)" nmerge - || true; \
		sudo chmod 777 $(XAUTH) && \
		docker run -it --name $$CONTAINER \
			--cap-add=SYS_NICE \
			--gpus all \
			--network host \
			--privileged \
			--ipc host \
			--shm-size=1gb \
			--volume=$(WORKSPACE):/home/$(USER_NAME)/$(PROJ_NAME):rw \
			--volume=$(XSOCK):$(XSOCK):rw \
			--volume=$(XAUTH):$(XAUTH):rw \
			--env=TERM=xterm-256color \
			--env="DISPLAY=$(DISPLAY)" \
			--env="XAUTHORITY=$(XAUTH)" \
			--env="QT_X11_NO_MITSHM=1" \
			--env="NVIDIA_VISIBLE_DEVICES=all" \
			--env="NVIDIA_DRIVER_CAPABILITIES=all" \
			--env="MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA" \
			$(DOCKER_IMAGE_BASE)-gpu \
			bash; \
	fi

# Attach to the running CPU container
exec_docker_cpu:
	docker exec -it $(CONTAINER_NAME_BASE)-cpu bash

# Attach to the running GPU container
exec_docker_gpu:
	docker exec -it $(CONTAINER_NAME_BASE)-gpu bash

# build ros packages
build:
	@set -e; \
	source /opt/ros/noetic/setup.bash; \
	export CC=gcc-11 CXX=g++-11; \
	ARCH=$$(dpkg-architecture -qDEB_HOST_MULTIARCH 2>/dev/null || echo x86_64-linux-gnu); \
	catkin build --cmake-args \
	  -DCMAKE_BUILD_TYPE=Release \
	  -DCMAKE_POLICY_VERSION_MINIMUM=3.5 \
	  -DCMAKE_SYSTEM_LIBRARY_PATH="/usr/lib/$$ARCH;/lib/$$ARCH;/usr/lib;/lib" \
	  -DCMAKE_CXX_FLAGS="-O2" \
	  -DENABLE_OSQP=ON

# clean build caches
clean:
	rm -r build devel logs .catkin_tools

# install packages which are not supported by rosdep
install_deps:
	sudo apt-get update && apt-get install -y \
		git \
		cmake \
		build-essential \
		pkg-config \
		psmisc \
		gcc-11
	bash shell/ensure_cmake.sh 3.18.0
	bash shell/install_osqp.sh
	bash shell/install_osqp_eigen.sh

killall:
	./shell/killall.sh

# record rosbag (all topics)
record:
	cd ${WORKSPACE}/rosbag; rosbag record -a

# play and check rosbag
## [shell 1] make view_rosbag
## [shell 2] rosbag play rosbag/xxx.bag
view_rosbag:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/rosbag_play.launch workspace:=${WORKSPACE}

# gazebo_world.launch
gazebo_world:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/gazebo_world.launch

# gmapping.launch
gmapping:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/gmapping.launch workspace:=${WORKSPACE}

# navigation.launch
navigation:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/navigation.launch workspace:=${WORKSPACE}

# navigation with nullspace_mpc
navigation_nullspace_mpc:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/navigation.launch workspace:=${WORKSPACE} local_planner:=nullspace_mpc

# navigation with nullspace_mpc (lite)
navigation_nullspace_mpc_lite:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	export OMT_WAIT_POLICY=PASSIVE &&\
	roslaunch launch/navigation.launch workspace:=${WORKSPACE} controller_mode:=lite  show_gazebo_gui:=false gazebo_headless:=true

# navigation with mppi
navigation_mppi:
	source /opt/ros/noetic/setup.bash && source ./devel/setup.bash &&\
	roslaunch launch/navigation.launch workspace:=${WORKSPACE} local_planner:=mppi_h

# evaluation demo with nullspace_mpc
eval_demo_nullspace_mpc:
	source $(WORKSPACE)/devel/setup.bash &&\
	mkdir -p result &&\
	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
	--agenda_yaml_path $(WORKSPACE)/data/eval_demo/agenda.yaml \
	--controller nullspace_mpc

# evaluation demo with mppi
eval_demo_mppi:
	source $(WORKSPACE)/devel/setup.bash &&\
	mkdir -p result &&\
	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
	--agenda_yaml_path $(WORKSPACE)/data/eval_demo/agenda.yaml \
	--controller mppi_h

# eval_demo_mppi_3d_a:
# 	source $(WORKSPACE)/devel/setup.bash &&\
# 	mkdir -p result &&\
# 	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
# 	--agenda_yaml_path $(WORKSPACE)/data/eval_demo/agenda.yaml \
# 	--controller mppi_3d_a

# eval_demo_mppi_3d_b:
# 	source $(WORKSPACE)/devel/setup.bash &&\
# 	mkdir -p result &&\
# 	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
# 	--agenda_yaml_path $(WORKSPACE)/data/eval_demo/agenda.yaml \
# 	--controller mppi_3d_b

eval_ten_small:
	source $(WORKSPACE)/devel/setup.bash &&\
	mkdir -p result &&\
	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
	--agenda_yaml_path $(WORKSPACE)/data/eval_ten/small/agenda_small.yaml \

eval_ten_large:
	source $(WORKSPACE)/devel/setup.bash &&\
	mkdir -p result &&\
	python3 $(WORKSPACE)/src/evaluation/mpc_nav_evaluator/scripts/run_evaluation.py \
	--agenda_yaml_path $(WORKSPACE)/data/eval_ten/large/agenda_large.yaml \

/home/yehuo/nullspace_mpc_ws/src/control/nullspace_mpc/CMakeLists.txt

复制代码 
cmake_minimum_required(VERSION 3.0.2)
project(nullspace_mpc)

# parameters
option(USE_OPENMP "USE_OPENMP" ON) # ON / OFF

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  visualization_msgs
  jsk_rviz_plugins
  nav_msgs
  roscpp
  tf2
  tf2_geometry_msgs
  tf2_ros
  grid_map_core
  grid_map_ros
  grid_map_filters
  grid_map_loader
  grid_map_msgs
  grid_map_rviz_plugin
  grid_map_visualization
  qp_solver_collection
  mpc_eval_msgs
)

# use eigen3 (matrix computation library)
find_package(Eigen3 REQUIRED)

# load openmp
if(USE_OPENMP)
  find_package(OpenMP REQUIRED)
  if(OpenMP_FOUND)
      message(WARNING "OpenMP found. Activate CPU acceleration.")
      set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
  endif()
  if(NOT OPENMP_FOUND)
      message(FATAL ERROR "Unable to find OpenMP library.")
  endif()
endif()

catkin_package(
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIR}
  # ${OPENMP_INCLUDE_DIR}
  /usr/local/include
)

## Declare a C++ library
add_library(nullspace_mpc_core SHARED src/nullspace_mpc_core.cpp)
add_library(hqp SHARED src/hqp.cpp /home/yehuo/nullspace_mpc_ws/src/QpSolverCollection/src/QpSolverCollection.cpp /home/yehuo/nullspace_mpc_ws/src/QpSolverCollection/src/QpSolverOsqp.cpp)

# hqp include directories
target_include_directories(hqp PUBLIC
  /usr/local/include/osqp      # OSQP include
  /usr/local/include
  /usr/local/share
  /home/yehuo/nullspace_mpc_ws/src/QpSolverCollection/src
)

## link openmp
if(USE_OPENMP)
  if (OPENMP_FOUND)
      if (TARGET OpenMP::OpenMP_CXX)
          target_link_libraries(nullspace_mpc_core OpenMP::OpenMP_CXX)
      endif()
  endif()
endif()

# Link OSQP to hqp
target_link_libraries(hqp
  /usr/local/lib/libosqp.so
  /usr/local/lib/libOsqpEigen.so
)

find_library(OSQP_LIB osqp HINTS /usr/local/lib)
find_library(OSQPEIGEN_LIB OsqpEigen HINTS /usr/local/lib)

target_link_libraries(hqp
  ${OSQP_LIB}
  ${OSQPEIGEN_LIB}
)

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/nullspace_mpc_node.cpp src/nullspace_mpc.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
  nullspace_mpc_core
  hqp
)

#############
## Install ##
#############

## Mark other files for installation (e.g. launch and bag files, etc.)
install(
  DIRECTORY
    launch
    config
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

"throw"报错

bash 复制代码 
"/home/yehuo/nullspace_mpc/src/openni2_camera/include/openni2_camera/openni2_device.h:67:48: error: ISO C++17 does not allow dynamic exception specifications 67 | OpenNI2Device(const std::string& device_URI) throw (OpenNI2Exception); "

直接删除或注释即可

右值"rvalue"错误

bash 复制代码 
error: taking address of rvalue [-fpermissive]
pos.object_id = static_cast<std::ostringstream*>(&(std::ostringstream() << max_id_))->str();

第七步：编译项目

bash 复制代码 
cd nullspace_mpc

sudo rosdep init # 如果已经显示初始化，则跳过这条指令，直接执行下一条

rosdep update

rosdep install -y --from-paths src --ignore-src --rosdistro noetic
相关推荐
小O的算法实验室13 天前
2023年IEEE TASE SCI2区，基于Dubins路径的多异构无人机动态灾情检测与验证集成分配，深度解析+性能实测
论文复现·智能算法·智能算法改进
小O的算法实验室18 天前
2025年COR SCI2区，基于近似细胞分解的能源高效无人机路径规划问题用于地质灾害监测，深度解析+性能实测
论文复现·智能算法·智能算法改进
小O的算法实验室18 天前
2024年ASOC SCI2区TOP，有效离散人工蜂群算法+变压器制造矩形切割问题，深度解析+性能实测
算法·论文复现·智能算法·智能算法改进
小O的算法实验室20 天前
2025年COR IOTJ SCI2区，灾后通信无人机基站位置优化和移动充电无人机路径规划，深度解析+性能实测
无人机·论文复现·智能算法·智能算法改进
小O的算法实验室22 天前
2023年IEEE IOTJ SCI1区TOP，动态环境下无人机目标覆盖任务路径规划，深度解析+性能实测
算法·无人机·论文复现·智能算法·智能算法改进
小O的算法实验室1 个月前
2025年KBS SCI1区TOP，矩阵差分进化算法+移动网络视觉覆盖无人机轨迹优化，深度解析+性能实测
算法·论文复现·智能算法改进
小O的算法实验室1 个月前
2024年Engineering SCI2区，面向工程管理的无人机巡检路径与调度，深度解析+性能实测
无人机·论文复现·智能算法·智能算法改进
小O的算法实验室1 个月前
2024年ESWA SCI1区TOP，自适应种群分配和变异选择差分进化算法iDE-APAMS，深度解析+性能实测
算法·论文复现·智能算法·智能算法改进
小O的算法实验室2 个月前
2025年ESWA SCI1区TOP，强化学习多目标灰狼算法MOGWO-RL+分布式混合流水车间调度，深度解析+性能实测
算法·论文复现·智能算法·智能算法改进
热门推荐
01UV 工具安装与国内镜像源配置指南02Spec-Kit 使用指南03GitHub 镜像站点0446个Nano-banana 精选提示词，持续更新中05UV安装并设置国内源06保姆级教程：手把手教你用Dify实现完美多轮对话（附Chatflow和提示词）07解决 WSL Ubuntu 中 /etc/resolv.conf 自动重置问题08KGG转MP3工具|非KGM文件|解密音频09Linux下V2Ray安装配置指南10jdk21下载、安装（Windows、Linux、macOS）