ROS2与Rviz2的贪吃蛇代码学习

Posted zhangrelay

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了ROS2与Rviz2的贪吃蛇代码学习相关的知识,希望对你有一定的参考价值。

参考网址:

github.com/Desperationis/rviz-snake/tree/v1.1.0

端午不休,学习代码。 

官方效果如下(引用):

 


ROS2 humble 

编译全过程:

zhangrelay@LAPTOP-5REQ7K1L:~/ros_ws$ cd rviz_snake/
zhangrelay@LAPTOP-5REQ7K1L:~/ros_ws/rviz_snake$ colcon build
Starting >>> snake_publisher
-- The C compiler identification is GNU 11.2.0
-- The CXX compiler identification is GNU 11.2.0
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Check for working C compiler: /usr/bin/cc - skipped
-- Detecting C compile features
-- Detecting C compile features - done
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++ - skipped
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Found ament_cmake: 1.3.2 (/opt/ros/humble/share/ament_cmake/cmake)
-- Found Python3: /usr/bin/python3.10 (found version "3.10.4") found components: Interpreter
-- Found rclcpp: 16.0.1 (/opt/ros/humble/share/rclcpp/cmake)
-- Found rosidl_generator_c: 3.1.3 (/opt/ros/humble/share/rosidl_generator_c/cmake)
-- Found rosidl_adapter: 3.1.3 (/opt/ros/humble/share/rosidl_adapter/cmake)
-- Found rosidl_generator_cpp: 3.1.3 (/opt/ros/humble/share/rosidl_generator_cpp/cmake)
-- Using all available rosidl_typesupport_c: rosidl_typesupport_fastrtps_c;rosidl_typesupport_introspection_c
-- Using all available rosidl_typesupport_cpp: rosidl_typesupport_fastrtps_cpp;rosidl_typesupport_introspection_cpp
-- Found rmw_implementation_cmake: 6.1.0 (/opt/ros/humble/share/rmw_implementation_cmake/cmake)
-- Found rmw_fastrtps_cpp: 6.2.1 (/opt/ros/humble/share/rmw_fastrtps_cpp/cmake)
-- Found OpenSSL: /usr/lib/x86_64-linux-gnu/libcrypto.so (found version "3.0.2")
-- Found FastRTPS: /opt/ros/humble/include
-- Using RMW implementation 'rmw_fastrtps_cpp' as default
-- Found visualization_msgs: 4.2.2 (/opt/ros/humble/share/visualization_msgs/cmake)
-- Found Curses: /usr/lib/x86_64-linux-gnu/libcurses.so
-- Found ament_lint_auto: 0.12.4 (/opt/ros/humble/share/ament_lint_auto/cmake)
-- Added test 'copyright' to check source files copyright and LICENSE
-- Added test 'cppcheck' to perform static code analysis on C / C++ code
-- Configured cppcheck include dirs: /home/zhangrelay/ros_ws/rviz_snake/src/snake_publisher/include/
-- Configured cppcheck exclude dirs and/or files:
-- Added test 'cpplint' to check C / C++ code against the Google style
-- Configured cpplint exclude dirs and/or files:
-- Added test 'lint_cmake' to check CMake code style
-- Added test 'uncrustify' to check C / C++ code style
-- Configured uncrustify additional arguments:
-- Added test 'xmllint' to check XML markup files
-- Configuring done
-- Generating done
-- Build files have been written to: /home/zhangrelay/ros_ws/rviz_snake/build/snake_publisher
[ 50%] Building CXX object CMakeFiles/publisher.dir/src/main.cpp.o
[100%] Linking CXX executable publisher
[100%] Built target publisher
-- Install configuration: ""
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/lib/snake_publisher/publisher
-- Set runtime path of "/home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/lib/snake_publisher/publisher" to ""
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/ament_index/resource_index/package_run_dependencies/snake_publisher
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/ament_index/resource_index/parent_prefix_path/snake_publisher
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/environment/ament_prefix_path.sh
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/environment/ament_prefix_path.dsv
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/environment/path.sh
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/environment/path.dsv
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/local_setup.bash
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/local_setup.sh
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/local_setup.zsh
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/local_setup.dsv
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/package.dsv
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/ament_index/resource_index/packages/snake_publisher
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/cmake/snake_publisherConfig.cmake
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/cmake/snake_publisherConfig-version.cmake
-- Installing: /home/zhangrelay/ros_ws/rviz_snake/install/snake_publisher/share/snake_publisher/package.xml
Finished <<< snake_publisher [30.4s]

rviz2中的蛇游戏;这是为了好玩,作为 ROS2 的介绍。

要求

目前,这仅rclcpp针对 ROS2 Galactic/Humble 进行了测试,尽管它很可能在任何稍旧的设备上都可以正常工作。此外,您需要安装 rviz2 和 ncurses(用于用户输入),通过sudo apt-get install libncurses-dev.

运行

首先,配置 ros2 环境。

  1. 通过进入根目录并运行colcon build.
  2. 源项目通过source install/setup.bash.
  3. 通过运行游戏ros2 run snake_publisher publisher。
  4. 在单独的终端中,运行rviz2 src/snake_publisher/rvizSetup.rviz.

这样,游戏就应该运行了。输入是在ros2 run运行的终端上进行的。

配置

目前实现了以下节点参数:

game_fps- 游戏更新的速率。
input_fps- 从队列中处理输入的速率。
snake_color_*- 蛇在其 RGB 组件中的颜色。
fruit_color_*- RGB 成分中水果的颜色。
grid_color_*- 网格在其 RGB 组件中的颜色。

限制/错误

没有提供启动文件(将来可以制作一个)
如果足够好来填满整个棋盘,游戏就会崩溃。
一段时间后,消息会慢慢延迟明显的数量;只需重新启动 rviz2 或关闭并打开标记显示(不要问为什么会这样)


核心代码:

#ifndef SNAKEGAME_HPP
#define SNAKEGAME_HPP

#include <memory>
#include <thread>
#include <functional>
#include <chrono>
#include <vector>
#include <ncurses.h>
#include <csignal>
#include <list>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/executor.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "std_msgs/msg/color_rgba.hpp"
#include "Markers.hpp"

using namespace std::chrono_literals;

/**
 * Snake.hpp
 *
 * This files holds classes that directly relate to running "Snake". Everything
 * is managed interally as simply points, and only get turned into cubes the
 * moment they are rendered; This allows for multiple possible ways to render
 * the game.
 */ 


/**
 * Details the color of each game piece.
*/ 
struct GamePieceColors 
	std_msgs::msg::ColorRGBA snakeColor;
	std_msgs::msg::ColorRGBA fruitColor;
	std_msgs::msg::ColorRGBA gridColor;
;


/**
 * Wrapper for GridMarker so that origin is at the topleft, x-axis is positive
 * to the right, y-axis is positive downwards, and the grid is drawn. This is
 * where points are turned into cubes.
*/ 
class SnakeGrid 
public:
	enum GRID_PIECES EMPTY, SNAKE, FRUIT;
private:
	int sideLength; 
	int worldXOffset, worldYOffset;
	std::vector<std::vector<GRID_PIECES>> gridElements;
public:
	/**
	 * Creates a grid that is at maximum size `sideLength` on each side.
	*/ 
	SnakeGrid(int sideLength) 
		this->sideLength = sideLength;
		worldXOffset = -sideLength / 2;
		worldYOffset = -sideLength / 2;
		
		for(int i = 0; i < sideLength; i++) 
			gridElements.push_back(std::vector<GRID_PIECES>());
			for(int j = 0; j < sideLength; j++) 
				gridElements[i].push_back(EMPTY);
			
		
	

	/**
	 * Determine whether a point is within the bounds of [0, side_length).
	*/ 
	bool InBounds(int x, int y) 
		return x < sideLength && x >= 0 && 
			y < sideLength && y >= 0;

	

	/**
	 * Reserve a specific spot to be drawn as a snake in the next frame, given
	 * that it is in bounds. If it is not in bounds, nothing will happen.
	*/ 
	void ReserveSnake(int x, int y) 
		if (this->InBounds(x, y)) 
			gridElements[y][x] = SNAKE;
		
	

	/**
	 * Reserve a specific spot to be drawn as a fruit in the next frame, given
	 * that it is in bounds. If it is not in bounds, nothing will happen.
	*/ 
	void ReserveFruit(int x, int y) 
		if (this->InBounds(x, y)) 
			gridElements[y][x] = FRUIT;
		
	

	/**
	 * Get the type of piece that is currently reserved. By default, every
	 * square on the grid is reserved EMPTY on every frame.
	*/ 
	GRID_PIECES GetReserved(int x, int y) 
		return gridElements[y][x];
	

	/**
	 * Returns the side length of the grid.
	*/ 
	int GetSideLength() 
		return sideLength;
	

	/**
	 * Draws the grid by iterating through all reserved portions and drawing a
	 * specific cube based on its type. For SNAKE and FRUIT, the square is
	 * elevated by 1 unit in order to give the apperance of 3D.
	*/ 
	void Draw(std::shared_ptr<MarkerPublisher> publisher, GamePieceColors colors) 
		GridMarker grid;
		for(size_t i = 0; i < gridElements.size(); i++) 
			for(size_t j = 0; j < gridElements[i].size(); j++) 
				GRID_PIECES type = gridElements[i][j];
				Cube cube;
				cube.SetPos(i + worldXOffset, j + worldYOffset, 1); 
				
				switch(type) 
				case EMPTY:
					cube.SetPos(i + worldXOffset, j + worldYOffset, 0); 
					cube.color = colors.gridColor;
					break;
				case SNAKE:
					cube.color = colors.snakeColor;
					break;
				case FRUIT:
					cube.color = colors.fruitColor;
					break;
				;

				grid.AddCube(cube);
			
		

		publisher->PublishMarker(grid);
	

	/**
	 * Completely clears the grid with EMPTY tiles.
	*/ 
	void Clear() 
		for(int i = 0; i < sideLength; i++) 
			for(int j = 0; j < sideLength; j++) 
				gridElements[i][j] = EMPTY;
			
		
	
;

struct Fruit 
	Fruit(int x, int y) 
		p.x = x;
		p.y = y;
	

	geometry_msgs::msg::Point p;
;

/**
 * Manager for controlling the spawning and erasing of fruits on the board.
*/ 
class FruitManager 
public:
	FruitManager() 
		requestedFruit = 0;
	

	/**
	 * Randomly spawn a single fruit that is not occupied by a SNAKE tile.
	 * TODO: Prevent this from being an infinite loop should a player good
	 * enough completely fill up the board.
	*/ 
	void SpawnFruit(SnakeGrid snakeGrid) 
		while(requestedFruit > 0) 
			int x = rand() % snakeGrid.GetSideLength();
			int y = rand() % snakeGrid.GetSideLength();

			if(snakeGrid.GetReserved(x, y) == SnakeGrid::EMPTY) 
				fruits.push_back(Fruit(x, y));
				snakeGrid.ReserveFruit(x, y);
				requestedFruit--;
			
		
	

	/**
	 * Request a single fruit to be drawn the next frame. Can be called
	 * multiple times for multiple fruits.
	*/ 
	void RequestFruit() 
		requestedFruit++;
	

	/**
	 * Try to eat a fruit at a specific point. If there is not fruit at that
	 * point, return false. If there is, return true and erase the fruit from
	 * existence.
	*/ 
	bool Eat(int x, int y)  
		for(size_t i = 0; i < fruits.size(); i++) 
			auto fruit = fruits[i];
			if(fruit.p.x == x && fruit.p.y == y) 
				fruits.erase(fruits.begin() + i);
				return true;
			
		

		return false;
	

	/**
	 * Get the list of points that occupy fruits.
	*/ 
	std::vector<Fruit> GetFruits() 
		return fruits;
	


private:
	std::vector<Fruit> fruits;

	// This variable holds the amount of requested fruit that should be spawned
	// on the next frame.
	int requestedFruit;
;

/**
 * The Snake. Body is completely represented by points.
*/
class Snake 
public:
	enum DIRECTION LEFT, RIGHT, UP, DOWN;

	Snake(int x, int y) 
		Respawn(x, y);
	

	std::list<geometry_msgs::msg::Point> GetBody() 
		return body;
	

	/**
	 * Updates the snake by a single frame. Essentially, it determines when it
	 * dies, how to move, and how to eat fruits.
	*/ 
	void Update(SnakeGrid& snakeGrid, FruitManager& fruitManager) 
		if(!dead) 
			geometry_msgs::msg::Point nextPoint;
			nextPoint = body.front();
			switch(currentDirection) 
			case LEFT:
				nextPoint.x -= 1;
				break;
			case RIGHT:
				nextPoint.x += 1;
				break;
			case UP:
				nextPoint.y -= 1;
				break;
			case DOWN:
				nextPoint.y += 1;
				break;
			;
			actualDirection = currentDirection;
			
			dead = WillDie(nextPoint.x, nextPoint.y, snakeGrid);

			if(!dead) 
				body.push_front(nextPoint);

				bool fruitEaten = fruitManager.Eat(nextPoint.x, nextPoint.y);

				if(!fruitEaten) 
					body.pop_back();
				
				else 
					fruitManager.RequestFruit();
				
			
		
	

	void SetDirection(DIRECTION dir) 
		currentDirection = dir;
	

	DIRECTION GetDirection() 
		return currentDirection;
	

	DIRECTION GetActualDirection() 
		return actualDirection;
	

	bool IsDead() 
		return dead;
	

	/**
	 * Checks whether or not the head is out of bounds our intersected its own
	 * body to determine if it died.
	*/ 
	bool WillDie(int x, int y, SnakeGrid& snakeGrid) 
		if(!snakeGrid.InBounds(x, y)) 
			return true;

		for(auto point : body) 
			if (point.x == x && point.y == y) 
				return true;
			
		

		return false;
	

	/**
	 * Respawn the snake at a specific point.
	*/ 
	void Respawn(int x, int y) 
		body.clear();
		
		geometry_msgs::msg::Point p;
		p.x = x;
		p.y = y;
		body.push_front(p);
		dead = false;

		currentDirection = RIGHT;
	

private:
	// First element is head, final element is tail.
	std::list<geometry_msgs::msg::Point> body;
	bool dead;
	DIRECTION currentDirection;

	// User input and the game are asynchronous as run at different hertz, so
	// this gives the direction the snake is headed based on the last frame so
	// that the user doesn't circumnavigate the input-checking code to prevent
	// crashing directly backwards.
	DIRECTION actualDirection; 
;


/**
 * ROS Node that actually runs the game. Due to not being able to create a
 * standard game loop with a delay, this node runs the loop via wall timers and
 * asynchronously receives input from the terminal via ncurses.
*/ 
class GameNode : public rclcpp::Node 
private:
	void OnGameFPSChange(const rclcpp::Parameter& p) 
		SetGameFPS(p.as_int());
	

	void OnInputFPSChange(const rclcpp::Parameter& p) 
		SetInputFPS(p.as_int());
	

	void SetGameFPS(int FPS) 
		int millisecondsToDelay = static_cast<int>(1000.0 / FPS);
		auto duration = std::chrono::duration<int, std::milli>(millisecondsToDelay);
		this->renderTimer = this->create_wall_timer(duration, std::bind(&GameNode::Loop, this));
	

	void SetInputFPS(int FPS) 
		int millisecondsToDelay = static_cast<int>(1000.0 / FPS);
		auto duration = std::chrono::duration<int, std::milli>(millisecondsToDelay);
		this->inputTimer = this->create_wall_timer(duration, std::bind(&GameNode::UserInput, this));
	

public:
	GameNode(std::shared_ptr<MarkerPublisher> publisher) : Node("game_node"), snake(5,5), snakeGrid(15) 
		snake.SetDirection(Snake::RIGHT);

		this->declare_parameter("game_fps", 12);
		this->declare_parameter("input_fps", 100);

		this->declare_parameter("snake_color_r", 0);
		this->declare_parameter("snake_color_g", 255);
		this->declare_parameter("snake_color_b", 0);

		this->declare_parameter("fruit_color_r", 255);
		this->declare_parameter("fruit_color_g", 0);
		this->declare_parameter("fruit_color_b", 0);

		this->declare_parameter("grid_color_r", 255);
		this->declare_parameter("grid_color_g", 255);
		this->declare_parameter("grid_color_b", 255);

		this->SetGameFPS(get_parameter("game_fps").as_int());
		this->SetInputFPS(get_parameter("input_fps").as_int());

		// Handle parameter changes
		paramSubscriber = std::make_shared<rclcpp::ParameterEventHandler>(this);
		gameFPSHandle = paramSubscriber->add_parameter_callback("game_fps", std::bind(&GameNode::OnGameFPSChange, this, std::placeholders::_1));
		inputFPSHandle = paramSubscriber->add_parameter_callback("input_fps", std::bind(&GameNode::OnInputFPSChange, this, std::placeholders::_1));

		this->publisher = publisher;

		fruitManager.RequestFruit();
		fruitManager.SpawnFruit(snakeGrid);
	

	/**
	 * Game loop.
	*/
	void Loop() 
		snake.Update(snakeGrid, fruitManager);
		for(auto body : snake.GetBody()) 
			snakeGrid.ReserveSnake(body.x, body.y);
		

		fruitManager.SpawnFruit(snakeGrid);
		for(auto fruit : fruitManager.GetFruits()) 
			snakeGrid.ReserveFruit(fruit.p.x, fruit.p.y);
		

		snakeGrid.Draw(publisher, GetColors());
		snakeGrid.Clear();
	

	/**
	 * Sole place for handling user input.
	*/ 
	void UserInput() 
		int c = getch();
		auto currentDirection = snake.GetActualDirection();
		if (c != -1) 
			if(c == 'w' && currentDirection != Snake::DOWN)
				snake.SetDirection(Snake::UP);
			if(c == 'a' && currentDirection != Snake::RIGHT)
				snake.SetDirection(Snake::LEFT);
			if(c == 's' && currentDirection != Snake::UP)
				snake.SetDirection(Snake::DOWN);
			if(c == 'd' && currentDirection != Snake::LEFT)
				snake.SetDirection(Snake::RIGHT);
			if(c == '\\n') 
				snake.Respawn(5,5);
		
	

	GamePieceColors GetColors() 
		GamePieceColors colors;
		colors.snakeColor.r = this->get_parameter("snake_color_r").as_int() / 255;
		colors.snakeColor.g = this->get_parameter("snake_color_g").as_int() / 255;
		colors.snakeColor.b = this->get_parameter("snake_color_b").as_int() / 255;
		colors.snakeColor.a = 1;
		
		colors.gridColor.r = this->get_parameter("grid_color_r").as_int() / 255;
		colors.gridColor.g = this->get_parameter("grid_color_g").as_int() / 255;
		colors.gridColor.b = this->get_parameter("grid_color_b").as_int() / 255;
		colors.gridColor.a = 1;

		colors.fruitColor.r = this->get_parameter("fruit_color_r").as_int() / 255;
		colors.fruitColor.g = this->get_parameter("fruit_color_g").as_int() / 255;
		colors.fruitColor.b = this->get_parameter("fruit_color_b").as_int() / 255;
		colors.fruitColor.a = 1;

		return colors;
	

private:
	Snake snake;
	SnakeGrid snakeGrid;
	std::shared_ptr<MarkerPublisher> publisher;
	FruitManager fruitManager;


	rclcpp::TimerBase::SharedPtr renderTimer, inputTimer;
	std::shared_ptr<rclcpp::ParameterEventHandler> paramSubscriber;
	std::shared_ptr<rclcpp::ParameterCallbackHandle> gameFPSHandle;
	std::shared_ptr<rclcpp::ParameterCallbackHandle> inputFPSHandle;
;

#endif

以上是关于ROS2与Rviz2的贪吃蛇代码学习的主要内容,如果未能解决你的问题,请参考以下文章

JS学习——贪吃蛇代码(简易版)

JS学习——贪吃蛇代码(简易版)

python学习笔记05:贪吃蛇游戏代码

贪吃蛇“大作战”

python游戏开发:打造最经典的贪吃蛇游戏

贪吃蛇的java代码分析