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

博客地址：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

git clone https://gitee.com/juanmv/osqp.git

cd osqp

mkdir build

cd build

cmake ..

sudo make install

安装 osqp-eigen

git clone https://gitee.com/juanmv/osqp-eigen.git

cd osqp-eigen

mkdir build

cd build

cmake ..

sudo make install

测试代码

git clone https://gitee.com/juanmv/osqp_test.git

cd osqp_test

mkdir build

cd build

cmake ..

make

./osqp_test

第二步：下载项目源码

git clone https://github.com/MizuhoAOKI/nullspace_mpc

第三步：自动下载依赖项

cd nullspace_mpc

sudo make install_deps

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

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

cd nullspace_mpc

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

rosdep update

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

第五步：Python 安装 jsk_data ROS 包

sudo apt-get update

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

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

/home/yehuo/nullspace_mpc_ws/Makefile

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

# 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”报错

"/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"错误

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

第七步：编译项目

cd nullspace_mpc

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

rosdep update

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