원래 erlerobotics document에서 도움을 받으려고 했으나 2019년 9월 23일인 어제 erlerobotics document에 접속해보니 서버가 터진건지 접속이 불가능했다.
그래서 이것저것 자료를 찾아보고 정리했다.
SLAM 작업환경 [Labtop]
OS : Ubuntu 16.06 LTS
ROS version : Kinetic
Compiler : catkin
Depth Camera : Intel realsene D435
Simulation 작업환경 [Desktop]
OS: Ubuntu 14.04 LTS
ROS version : Indigo
Compiler : catkin
Depth Camera : Kinect Sensor
이번 내용은 erle github와 ros q&a에 제공된 코드를 이용하였다.
erle github
https://github.com/erlerobot/ros_erle_takeoff_land
ros q&a
https://gist.github.com/anonymous/b9ff7b1a24c41104073d2b56917351a1
기본적으로 드론을 단순히 조종하려는 목적에서 velocity와 time을 이용하여 드론을 control하였다.
( 추가적으로 드론을 회전시키려 해보았으나 근본적인 문제가 있는지 불가능 하였다. 이 내용은 다음의 thread에서 다뤄진 바 있다. https://github.com/mavlink/mavros/issues/627 )
1. erle-copter take off
erle github에서는 기본적으로 mavros message를 이용해 erle-copter를 띄울 수 있는 source를 제공한다. code는 굉장히 단순하여 보기만 하면 금방 이해될 것이다. 그래도 한번 리뷰하고 시작하겠다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
|
#include <cstdlib>
#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
/////review///// mavros_msgs가 include된걸 확인할 수 있다. mavros_msgs는 지상 및 비행 로봇 등을 컨트롤 할 수 있는 message들을 포함하고 있다.
int main(int argc, char **argv)
{
int rate = 10;
ros::init(argc, argv, "mavros_takeoff");
ros::NodeHandle n;
ros::Rate r(rate);
////////////////////////////////////////////
/////////////////GUIDED/////////////////////
////////////////////////////////////////////
ros::ServiceClient cl = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
mavros_msgs::SetMode srv_setMode;
srv_setMode.request.base_mode = 0;
srv_setMode.request.custom_mode = "GUIDED";
if(cl.call(srv_setMode)){
ROS_INFO("setmode send ok %d value:", srv_setMode.response.success);
}else{
ROS_ERROR("Failed SetMode");
return -1;
}
/////review///// drone을 조종할 때 mode를 설정한다. GUIDED 모드로 설정하게 되며, "명령을 받아 수행하는 모드"라고 생각하면 된다.
////////////////////////////////////////////
///////////////////ARM//////////////////////
////////////////////////////////////////////
ros::ServiceClient arming_cl = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
mavros_msgs::CommandBool srv;
srv.request.value = true;
if(arming_cl.call(srv)){
ROS_INFO("ARM send ok %d", srv.response.success);
}else{
ROS_ERROR("Failed arming or disarming");
}
/////review///// arm은 드론의 프로펠러 부분으로써 arming이란 프로펠러를 돌리는 동작을 말한다.
////////////////////////////////////////////
/////////////////TAKEOFF////////////////////
////////////////////////////////////////////
ros::ServiceClient takeoff_cl = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
mavros_msgs::CommandTOL srv_takeoff;
srv_takeoff.request.altitude = 1;
srv_takeoff.request.latitude = 0;
srv_takeoff.request.longitude = 0;
srv_takeoff.request.min_pitch = 0;
srv_takeoff.request.yaw = 0;
if(takeoff_cl.call(srv_takeoff)){
ROS_INFO("srv_takeoff send ok %d", srv_takeoff.response.success);
}else{
ROS_ERROR("Failed Takeoff");
}
/////review///// CommandTOL은 Command Takeoff Or Land 로써 이륙 및 착륙에 대한 message이다. 이 때, altitude는 고도, latitude는 위도, longitude는 경도를 의미한다.
////////////////////////////////////////////
/////////////////DO STUFF///////////////////
////////////////////////////////////////////
sleep(10);
/////review///// sleep(10); 을 통해 드론이 이륙할 때 까지 기다린다. sleep(10); 이 없으면 이륙에 실패하니 조금만 기다리도록 하자. 추가적으로 이륙에 성공하고 나면 어떤 작업을 수행할지 이 부분에 코딩하면 된다.
////////////////////////////////////////////
///////////////////LAND/////////////////////
////////////////////////////////////////////
ros::ServiceClient land_cl = n.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
mavros_msgs::CommandTOL srv_land;
srv_land.request.altitude = 1;
srv_land.request.latitude = 0;
srv_land.request.longitude = 0;
srv_land.request.min_pitch = 0;
srv_land.request.yaw = 0;
if(land_cl.call(srv_land)){
ROS_INFO("srv_land send ok %d", srv_land.response.success);
}else{
ROS_ERROR("Failed Land");
}
/////review///// CommandTOL은 Command Takeoff Or Land 로써 이륙 및 착륙에 대한 message이다. 이 때, altitude는 고도, latitude는 위도, longitude는 경도를 의미한다.
while (n.ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
}
| cs |
위 소스는 다음과 같은 방법으로 가져올 수 있다.
(1) workspace로 이동
$ cd ~/simulation/ros_catkin_ws/src(2) download package
$ git clone https://github.com/erlerobot/ros_erle_takeoff_land(3) compile
$ cd ..$ catkin_make
그러면 명령어를 통해 takeoff 작업을 수행해 보겠다.
( 각 명령어는 다른 터미널에서 실행되어야 한다. )
(1) Launch ArduCopter
(Ref. http://docs.erlerobotics.com/simulation/vehicles/erle_copter/tutorial_1)
$ source ~/simulation/ros_catkin_ws/devel/setup.bash
$ cd ~/simulation/ardupilot/ArduCopter
$ ../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo
(2) Launch Spawn
(Ref. http://docs.erlerobotics.com/simulation/vehicles/erle_copter/tutorial_1) $ source ~/simulation/ros_catkin_ws/devel/setup.bash
$ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_spawn.launch
(3) Launch mavlink_bridge.launch
$ source ~/simulation/ros_catkin_ws/devel/setup.bash$ roslaunch mavros mavlink_bridge.launch
(4) Run ros_erle_takeoff_land
$ rosrun ros_erle_takeoff_land ros_erle_takeoff_land
2. Writing moving code
이제 위 takeoff source에 code를 추가하여 copter를 움직일 것이다.
위에서 본 source에서 /////DO STUFF///// 부분만 수정하여 리뷰해볼 것이다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
////////////////////////////////////////////
/////////////////DO STUFF///////////////////
////////////////////////////////////////////
sleep(10);
// setting to move drone
ros::Publisher set_vel_pub = n.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);
mavros_msgs::PositionTarget pos;
pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
pos.type_mask = mavros_msgs::PositionTarget::IGNORE_PX | mavros_msgs::PositionTarget::IGNORE_PY |
mavros_msgs::PositionTarget::IGNORE_PZ | mavros_msgs::PositionTarget::IGNORE_AFX |
mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ |
mavros_msgs::PositionTarget::IGNORE_YAW | mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
/////review///// PositionTarget은 position과 acceleration, velocity등을 이용해 로봇을 컨트롤 할 수 있도록 하는 message이다. 위 내용은 PositionTarget message의 기본적인 setting이다. pos.position.x = 0.0f;
pos.position.y = 0.0f;
pos.position.z = 0.0f;
pos.acceleration_or_force.x = 0.0f;
pos.acceleration_or_force.y = 0.0f;
pos.acceleration_or_force.z = 0.0f;
pos.velocity.x = 0.0f;
pos.velocity.y = 0.0f;
pos.velocity.z = 0.0;
pos.yaw = 0.0f;
pos.yaw_rate = 0.0f;
if (set_vel_pub)
{
ROS_INFO("zero velocity");
for (int i = 100; ros::ok() && i > 0; --i)
{
set_vel_pub.publish(pos);
ros::spinOnce();
// rate.sleep();
ros::Duration(0.01).sleep();
}
ROS_INFO("Done with zero velocity set");
}
/////review///// 모든 값들을 0으로 줌으로써 로봇의 초기 정지 상태를 설정해 안정적으로 시작할 수 있도록 한다.
// go forward for 50[sec]
pos.velocity.x = -0.4f;
pos.velocity.y = 0.0f;
pos.velocity.z = 0.0f;
pos.yaw = 1.57;
pos.yaw_rate = 1;
if (set_vel_pub)
{
for (int i = 5000; ros::ok() && i > 0; --i)
{
set_vel_pub.publish(pos);
ros::spinOnce();
// rate.sleep();
ros::Duration(0.01).sleep();
}
ROS_INFO("Done");
}
/////review///// pos.velocity.x의 값을 줌으로써 drone이 이동하게 된다. 0.01초를 5000번정도 반복하므로, 약 50초 정도 forward 방향으로 이동하게 된다.
// stop for 1[sec]
pos.velocity.x = 0.0f;
pos.velocity.y = 0.0f;
pos.velocity.z = 0.0f;
pos.yaw = 1.57;
pos.yaw_rate = 1;
if (set_vel_pub)
{
for (int i = 1000; ros::ok() && i > 0; --i)
{
set_vel_pub.publish(pos);
ros::spinOnce();
// rate.sleep();
ros::Duration(0.01).sleep();
}
ROS_INFO("Done");
}
/////review///// 갑작스럽게 방향을 바꾸면 기체가 흔들릴 수 있으므로 10초정도 멈추어 준다.
// go backward for 50[sec]
pos.velocity.x = 0.4f;
pos.velocity.y = 0.0f;
pos.velocity.z = 0.0f;
pos.yaw = -1.57;
pos.yaw_rate = 1;
if (set_vel_pub)
{
for (int i = 5000; ros::ok() && i > 0; --i)
{
set_vel_pub.publish(pos);
ros::spinOnce();
// rate.sleep();
ros::Duration(0.01).sleep();
}
ROS_INFO("Done");
}
/////review///// pos.velocity.x의 값을 줌으로써 drone이 이동하게 된다. 0.01초를 5000번정도 반복하므로, 약 50초 정도 backward 방향으로 이동하게 된다.
// stop for 1[sec]
pos.velocity.x = 0.0f;
pos.velocity.y = 0.0f;
pos.velocity.z = 0.0f;
pos.yaw = 1.57;
pos.yaw_rate = 1;
if (set_vel_pub)
{
for (int i = 1000; ros::ok() && i > 0; --i)
{
set_vel_pub.publish(pos);
ros::spinOnce();
// rate.sleep();
ros::Duration(0.01).sleep();
}
ROS_INFO("Done");
}
/////review///// 갑작스럽게 방향을 바꾸면 기체가 흔들릴 수 있으므로 10초정도 멈추어 준다. | cs |
위 내용을 추가하여 1번에서 실행한 명령어를 반복해주면 다음과 같은 결과를 얻을 수 있다.
(1) Launch ArduCopter
(Ref. http://docs.erlerobotics.com/simulation/vehicles/erle_copter/tutorial_1)
$ source ~/simulation/ros_catkin_ws/devel/setup.bash
$ cd ~/simulation/ardupilot/ArduCopter
$ ../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo
(2) Launch Spawn
(Ref. http://docs.erlerobotics.com/simulation/vehicles/erle_copter/tutorial_1) $ source ~/simulation/ros_catkin_ws/devel/setup.bash
$ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_spawn.launch
(3) Launch mavlink_bridge.launch
$ source ~/simulation/ros_catkin_ws/devel/setup.bash$ roslaunch mavros mavlink_bridge.launch
(4) Run ros_erle_takeoff_land
$ rosrun ros_erle_takeoff_land ros_erle_takeoff_land
영상을 첨부한다.
드론이 잘 움직이는 것을 확인할 수 있었다. 하지만 여전히 드론의 회전문제는 해결하지 못했다.
PositionTarget의 yaw와 yaw_rate를 이용한다고는 하는데 어떤 값을 주어도 전혀 작동이 되지 않는다.
이부분이 아쉽지만 일단 실험환경은 완성되었다.
댓글 없음:
댓글 쓰기