# Rotors Simulator Usage

## 1 创建功能包

切换到工作空间的`src`目录下，创建`rotors_control_test`功能包

```shell
# 工作空间目录下的src
cd catkin_ws/src
# 创建功能包，包名后面的参数是相关依赖
catkin_create_pkg rotors_control_test roscpp rospy std_msgs 
# sensor_msgs geometry_msgs mav_msgs gazebo_msgs
```

## 2 修改相关功能包相关依赖

如果创建功能包的时候，少或错误添加相关依赖，可以打开`CMakeLists.txt`，修改相关内容:

```shell
find_package(catkin REQUIRED COMPONETS
	roscpp
	rospy
	std_msgs
	# 以上三个是常用的依赖，以下是本功能包需要用到的额外的依赖
	sensor_msgs
	geometry_msgs
	mav_msgs
	gazebo_msgs
)
```

同时需要修改`package.xml`文件，在其中添加:

```xml
<depend>gazebo_msgs</depend>
<depend>gazebo_plugins</depend>
<depend>geometry_msgs</depend>
<depend>joy</depend>
<depend>mav_msgs</depend>
<depend>rotors_gazebo_plugins</depend>
<depend>sensor_msgs</depend>
<depend>xacro</depend>
```

添加依赖XXX，在功能包中即可以使用XXX/XXX的消息类型，如`mav_msgs`，能够让功能包使用`mav_msgs/xxx`类型的消息。

## 3 创建源文件并修改`CMakeLists.txt`

在`rotors_control_test/src`下创建`rotors_control_node.cpp`文件，并修改`CMakeLists.txt`：

```shell
add_executable(
	rotors_control_node src/rotors_control_node.cpp
)

target_link_libraries(
	rotors_control_node ${catkin_LIBRARIES}
)
```

## 4 编写源文件代码

### 代码思路

1. 初始化 ROS 节点；
2. 创建`position_sub`订阅无人机位置信息话题：
   1. 话题名为`/odometry_sensor1/position`，可以通过`rqt`查询其他话题名称及对应的消息数据类型；
   2. 创建`updateUAVPosition`回调函数，位置信息类型为`geometry_msgs::PointStamped`；
   3. **注意**：当`GPS`初始化成功并能够接收到位置信息时，才开始执行任务；
3. 创建名为`trajectory_pub`的控制指令`publisher`:
   1. 控制无人机移动可使用`rotors_simulator`包中的控制节点：当前`pulisher`中发布的话题即为`rotors_simulator`中已定义好的话题；
   2. 值得注意的是，`rotors_simulator`中控制节点仅对无人机位置进行控制，即发送**指定位置**。同时，在起点与目标位置相距较远时，无人机会侧翻（对于仿真是没有什么影响，但对于将算法投入实际使用时，物理机则需要更多考量）；
   3. 为避免无人机侧翻，平滑移动轨迹仍十分重要。

### 相关API

`mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw`

```cpp
// Convenience method to quickly create a trajectory from a single waypoint.
```

### 源代码

```cpp
#include <ros/ros.h>
#include <chrono>
#include <thread>
#include <Eigen/Core>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>

#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

ros::Publisher trajectory_pub;
geometry_msgs::PointStamped cur_pos_msg;

float linear_smoothing_navigation_step = 2;
bool gps_init_ok = false;
bool take_off_ok = false;
int tasks_ok = 0;
Eigen::Vector3d home_pos;

/*
 *Description:
 *	when gps position changed, we update related parameters.
 *Parameters:
 *	msg : current position message
 */
void updateUAVPosition(const geometry_msgs::PointStamped& msg) {
	if(!gps_init_ok) {
		// prepare to take off
		gps_init_ok = true;
		// record current position
		home_pos[0] = msg.point.x;
		home_pos[1] = msg.point.y;
		home_pos[2] = msg.point.z;
		ROS_INFO("Home Position is <%f, %f, %f>", msg.point.x, msg.point.y, msg.point.z);
	}
	cur_pos_msg = msg;
	// ROS_INFO("Current Position is <%f, %f, %f>", msg.point.x, msg.point.y, msg.point.z);
}

/*
 *Description:
 *	calculate the distance between current position and target position
 *Parameters:
 *	target_pos : destination position	
 *Return:
 *	double : distance 
 */
double getDistanceToDestination(const Eigen::Vector3d& target_pos) {
	double distance = 0.0;
	distance = pow((target_pos[0] - cur_pos_msg.point.x), 2) + 
			   pow((target_pos[1] - cur_pos_msg.point.y), 2) +
			   pow((target_pos[2] - cur_pos_msg.point.z), 2);
	return sqrt(distance);
}

/*
 *Description:
 *	
 *Parameters:
 *	target_pos : destination position	
 *	max_err : max error (default = 0.0001)
 *Return:
 *	true : arrival
 *	false : not arrival
 */
bool isReachDestination(const Eigen::Vector3d& target_pos, float max_err = 0.2) {
	double cur_err = getDistanceToDestination(target_pos);
	return cur_err < max_err;
}


bool linearSmoothingNavigationTask(const Eigen::Vector3d& target_pos) {
	trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
	trajectory_msg.header.stamp = ros::Time::now();
	if(isReachDestination(target_pos)) {
		return true;
	}
	double dist = getDistanceToDestination(target_pos);
	Eigen::Vector3d next_pos;
	if(dist < linear_smoothing_navigation_step) {
		next_pos = target_pos;
	} else {
		next_pos[0] = cur_pos_msg.point.x + 
			(target_pos[0] - cur_pos_msg.point.x) / dist * linear_smoothing_navigation_step;
		next_pos[1] = cur_pos_msg.point.y + 
			(target_pos[1] - cur_pos_msg.point.y) / dist * linear_smoothing_navigation_step;
		next_pos[2] = cur_pos_msg.point.z + 
			(target_pos[2] - cur_pos_msg.point.z) / dist * linear_smoothing_navigation_step;
	}
	double target_yaw = 0.0;
	mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(target_pos, target_yaw, &trajectory_msg);
	trajectory_pub.publish(trajectory_msg);
	
	ROS_INFO("point %d", tasks_ok);
	return false;	
}

/*
 *Description:
 *	UAV reach the height of taking off.
 *Parameters:
 *	altitude : the height of taking off
 */
bool takeOff(float altitude) {
	trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
	trajectory_msg.header.stamp = ros::Time::now();
	static Eigen::Vector3d target_pos(cur_pos_msg.point.x, cur_pos_msg.point.y, altitude);
	double target_yaw = 0.0;
	if(isReachDestination(target_pos)) {
		return true;
	}

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(target_pos, target_yaw, &trajectory_msg);
	trajectory_pub.publish(trajectory_msg);
	return false;
}

/*
 *Description:
 *	Go back to the origin position. UAV move to home_pos with current altitude. Then UAV lands.
 */
void goHome() {
	static Eigen::Vector3d home_pos_bar(home_pos[0], 
										home_pos[1],
										cur_pos_msg.point.z);
	static bool home_pos_ok = false;
	if(!home_pos_ok) {
		home_pos_ok = linearSmoothingNavigationTask(home_pos_bar);	
	} else {
		linearSmoothingNavigationTask(home_pos);
		ROS_INFO("UAV aready go back.");
	}
}

int main(int argc, char *argv[]) {
	// set local code
	setlocale(LC_ALL, "");
	// ros node init
	ros::init(argc, argv, "rotors_control_node");
	ros::NodeHandle nh;
	// create a private node for accessing node parameters
	ros::NodeHandle nh_private("~");

	std::string uav_name = "";
	ros::param::get("~mav_name", uav_name);

	// subcribe topic
	// uav position info : /odometry_sensor1/position
	ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUAVPosition);
	// global variable
	trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

	// wait 5s for gazebo successfully startup
	ros::Duration(5.0).sleep();

	// make gazebo auto-play
	std_srvs::Empty srv;
	bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);

	// try to make gazebo auto-play
	int i = 0;
	while(i <= 10 && !unpaused) {
		ROS_INFO("Wait for 1 second before trying to unpause Gazebo again!");
		std::this_thread::sleep_for(std::chrono::seconds(1));
		unpaused = ros::service::call("/gazebo/unpause_physics", srv);
		++i;
	}
	
	// if Gazebo is not auto-play, then exit
	if(!unpaused) {
		ROS_FATAL("Fail to wake up Gazebo.");
		return -1;	
	} else {
		ROS_INFO("Unpaused the Gazebo simulation.");
	}

	// trajectory
	std::vector<Eigen::Vector3d> path;
	path.push_back(Eigen::Vector3d(5.f, 5.f, 5.f));
	path.push_back(Eigen::Vector3d(-5.f, 5.f, 5.f));
	path.push_back(Eigen::Vector3d(-5.f, -5.f, 5.f));
	path.push_back(Eigen::Vector3d(5.f, -5.f, 5.f));
	path.push_back(Eigen::Vector3d(5.f, 5.f, 5.f));
	ROS_INFO("Trajectory has %d points.", path.size());

	ros::Rate loop_rate(10);
	while(ros::ok()) {
		if(gps_init_ok && !take_off_ok) {
			take_off_ok = takeOff(3);
		} else if(take_off_ok && tasks_ok < path.size()) {
			ROS_INFO("Move");
			if(tasks_ok < path.size()) {
				bool tmp = linearSmoothingNavigationTask(path[tasks_ok]);
				if(tmp) {
					tasks_ok++;
				}
			}
		} else if(tasks_ok >= path.size()) {
			goHome();
		}
		ROS_INFO("take off : %d, task number : %d", take_off_ok, tasks_ok);
		ros::spinOnce();
		loop_rate.sleep();
	}

	return 0;
}
```

## 5 `launch`文件配置

在`rotors_control_test`目录下创建`launch`文件夹，在`launch`文件夹下创建`rotors_control_test.launch`文件

```xml
<launch>
    <arg name="mav_name" default="firefly"/>
    <arg name="world_name" default="basic"/>
    <arg name="enable_logging" default="false" />
    <arg name="enable_ground_truth" default="true" />
    <arg name="log_file" default="$(arg mav_name)" />
    <arg name="debug" default="false"/>
    <arg name="gui" default="true"/>
    <arg name="paused" default="false"/>
    <!-- The following line causes gzmsg and gzerr messages to be printed to the console
        (even when Gazebo is started through roslaunch) -->
    <arg name="verbose" default="false"/>

    <!--Run Gazebo-->
    <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
    <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="paused" value="$(arg paused)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="verbose" value="$(arg verbose)"/>
    </include>

    <!--Run UAV model and control node-->
    <group ns="$(arg mav_name)">
        <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
        <arg name="mav_name" value="$(arg mav_name)" />
        <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
        <arg name="enable_logging" value="$(arg enable_logging)" />
        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
        <arg name="log_file" value="$(arg log_file)"/>
        </include>

        <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
        <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
        <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
        <remap from="odometry" to="odometry_sensor1/odometry" />
        </node>

        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

        <node name="rotors_control_node" pkg="rotors_control_test" type="rotors_control_node" output="screen">
            <param name="mav_name" type="string" value="$(arg mav_name)"/>
        </node>  
    </group>
</launch>
```

## 6 编译并运行

当前文件目录结构

```shell
| -- catkin_ws
	| -- build
	| -- devel
	| -- src
    	| -- rotors_control_test
        	| -- launch
            	| -- rotors_control_test.launch
			| -- src
            	| -- rotors_control_test.cpp
			CMakeLists.txt
		| -- mav_comm
		| -- rotors_simulator
        CMakeLists.txt
```

先切换到工作空间下，編譯，運行

```shell
# 先切换到工作空间下
cd ./catkin_ws

# 编译
catkin_make

# 运行
source ./devel/setup.bash
roslaunch rotors_control_test rotors_control_test.launch
```

## Reference

1.


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://merlotliu.gitbook.io/ros-learning/my-notes/rotors-simulator/rotors-simulator-usage.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.