자율주행 로봇을 제작하는 것을 목표로 하지만, 제작 과정에서 실험을 위해 직접 조작이 필요한 경우가 있다.
그 때 사용하는 패키지를 다운받아 수정하여 사용해 보자.
사용버전. 노트북. 모터
˙ Samsung Ultrabook i7
˙ Ubuntu 18.04.6 LTS
˙ Ros Melodic
˙ IG-52GM+Encoder 04TYPE (24V)
단계
1. 패키지 다운
2. 패키지 실행
3. subscriber 파일 생성 및 코드 작성
4. package.xml 수정
5. 입력내용 확인
6. 패키지 publish 코드 수정
7. Subscriber 수정
8. 수정된 코드 동작 확인
9. OpenCR 이용해 모터 구동
1. 패키지 다운
먼저 catkin_ws/src 로 이동 한다. 이후 teleop_twist를 설치해 준다.
$ cw
$ cd src
$ git clone https://github.com/methylDragon/teleop_twist_keyboard_cpp.git
$ cd ..
$ cm
$ source devel/setup.bash
2. 패키지 실행
패키지가 잘 설치되었는지 확인하기 위해 패키지를 실행시켜 본다. 터미널 2개를 띄워 실행한다.
#1st Terminal
$ roscore
#2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard
이상없이 실행됨을 알 수 있다.
3. subscriber 파일 생성 및 코드 작성
키보드로 입력이 됨은 확인이 되었으니, 입력받은 내용을 확인해 보자.
teleop_twist_keyboard_cpp 패키지 안에 key_sub라는 이름으로 subcriber를 만들어 보자.
$ cw
$ cd src/teleop_twist_keyboard_cpp/src
$ gedit key_sub.cpp
teleop_twist가 geometry_msgs/Twist를 사용하므로 각 변수들의 값을 출력하는 코드를 작성해 본다.
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
void response(const geometry_msgs::Twist::ConstPtr& msg)
{
ROS_INFO("linear.x = %f",msg->linear.x);
ROS_INFO("linear.y = %f",msg->linear.y);
ROS_INFO("linear.z = %f",msg->linear.z);
ROS_INFO("angular.x = %f",msg->angular.x);
ROS_INFO("angular.y = %f",msg->angular.y);
ROS_INFO("angular.z = %f",msg->angular.z);
ROS_INFO("=======================");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "key_sub");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/cmd_vel",100,response);
ros::spin();
return 0;
}이후 catkin_make를 통해 오류를 확인하고 수정한다. 오류가 발생하지 않으면, 넘어간다.
$ cm
4. CMakeList.txt 수정
패키지에 subscriber를 추가했기 때문에 해당 내용을 추가해 주도록 한다.
package.xml은 수정하지 않아도 된다.
아래는 원본 코드이다.
cmake_minimum_required(VERSION 2.8.3)
project(teleop_twist_keyboard_cpp)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
)
catkin_package(
INCLUDE_DIRS src
CATKIN_DEPENDS roscpp geometry_msgs
)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)
target_link_libraries(teleop_twist_keyboard ${catkin_LIBRARIES})
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
cmake_minimum_required(VERSION 2.8.3)
project(teleop_twist_keyboard_cpp)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
)
catkin_package(
INCLUDE_DIRS src
CATKIN_DEPENDS roscpp geometry_msgs
)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)
add_executable(key_sub src/key_sub.cpp)
add_dependencies(teleop_twist_keyboard ${teleop_twist_keyboard_cpp_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(key_sub ${teleop_twist_keyboard_cpp_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(teleop_twist_keyboard ${catkin_LIBRARIES})
target_link_libraries(key_sub ${catkin_LIBRARIES})
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
아래는 수정된 코드이다.
cmake_minimum_required(VERSION 2.8.3)
project(teleop_twist_keyboard_cpp)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
)
catkin_package(
INCLUDE_DIRS src
CATKIN_DEPENDS roscpp geometry_msgs
)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)
add_executable(key_sub src/key_sub.cpp)
add_dependencies(teleop_twist_keyboard ${teleop_twist_keyboard_cpp_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(key_sub ${teleop_twist_keyboard_cpp_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(teleop_twist_keyboard ${catkin_LIBRARIES})
target_link_libraries(key_sub ${catkin_LIBRARIES})
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")코드는 수정 이후에 catkin_make 해준다.
5. 입력내용 확인
터미널 창 3개를 띄워서 입력한 값이 출력되는걸 확인한다.
#1st Terminal
$ roscore
#2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard
#3rd Terminal
$ rosrun teleop_twist_keyboard_cpp key_sub
입력한 값이 출력됨을 확인할 수 있었다.
6. 패키지 publish 코드 수정
각각의 키 입력에 대한 작성된 값이 원하는 바와 다르므로 수정하여 사용하도록 한다.
w/a/s/d 를 통해 움직이고 i/o를 통해 속력을, k/l을 통해 회전 정도를 조절한다.
w/a/s/d 의 경우는 1/0 으로 신호만 준다.
아래는 원본 코드이다.
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <map>
// Map for movement keys
std::map<char, std::vector<float>> moveBindings
{
{'i', {1, 0, 0, 0}},
{'o', {1, 0, 0, -1}},
{'j', {0, 0, 0, 1}},
{'l', {0, 0, 0, -1}},
{'u', {1, 0, 0, 1}},
{',', {-1, 0, 0, 0}},
{'.', {-1, 0, 0, 1}},
{'m', {-1, 0, 0, -1}},
{'O', {1, -1, 0, 0}},
{'I', {1, 0, 0, 0}},
{'J', {0, 1, 0, 0}},
{'L', {0, -1, 0, 0}},
{'U', {1, 1, 0, 0}},
{'<', {-1, 0, 0, 0}},
{'>', {-1, -1, 0, 0}},
{'M', {-1, 1, 0, 0}},
{'t', {0, 0, 1, 0}},
{'b', {0, 0, -1, 0}},
{'k', {0, 0, 0, 0}},
{'K', {0, 0, 0, 0}}
};
// Map for speed keys
std::map<char, std::vector<float>> speedBindings
{
{'q', {1.1, 1.1}},
{'z', {0.9, 0.9}},
{'w', {1.1, 1}},
{'x', {0.9, 1}},
{'e', {1, 1.1}},
{'c', {1, 0.9}}
};
// Reminder message
const char* msg = R"(
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
)";
// Init variables
float speed(0.5); // Linear velocity (m/s)
float turn(1.0); // Angular velocity (rad/s)
float x(0), y(0), z(0), th(0); // Forward/backward/neutral direction vars
char key(' ');
// For non-blocking keyboard inputs
int getch(void)
{
int ch;
struct termios oldt;
struct termios newt;
// Store old settings, and copy to new settings
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
// Make required changes and apply the settings
newt.c_lflag &= ~(ICANON | ECHO);
newt.c_iflag |= IGNBRK;
newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
newt.c_cc[VMIN] = 1;
newt.c_cc[VTIME] = 0;
tcsetattr(fileno(stdin), TCSANOW, &newt);
// Get the current character
ch = getchar();
// Reapply old settings
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
int main(int argc, char** argv)
{
// Init ROS node
ros::init(argc, argv, "teleop_twist_keyboard");
ros::NodeHandle nh;
// Init cmd_vel publisher
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
// Create Twist message
geometry_msgs::Twist twist;
printf("%s", msg);
printf("\rCurrent: speed %f\tturn %f | Awaiting command...\r", speed, turn);
while(true){
// Get the pressed key
key = getch();
// If the key corresponds to a key in moveBindings
if (moveBindings.count(key) == 1)
{
// Grab the direction data
x = moveBindings[key][0];
y = moveBindings[key][1];
z = moveBindings[key][2];
th = moveBindings[key][3];
printf("\rCurrent: speed %f\tturn %f | Last command: %c ", speed, turn, key);
}
// Otherwise if it corresponds to a key in speedBindings
else if (speedBindings.count(key) == 1)
{
// Grab the speed data
speed = speed * speedBindings[key][0];
turn = turn * speedBindings[key][1];
printf("\rCurrent: speed %f\tturn %f | Last command: %c ", speed, turn, key);
}
// Otherwise, set the robot to stop
else
{
x = 0;
y = 0;
z = 0;
th = 0;
// If ctrl-C (^C) was pressed, terminate the program
if (key == '\x03')
{
printf("\n\n . .\n . |\\-^-/| . \n /| } O.=.O { |\\\n\n CH3EERS\n\n");
break;
}
printf("\rCurrent: speed %f\tturn %f | Invalid command! %c", speed, turn, key);
}
// Update the Twist message
twist.linear.x = x * speed;
twist.linear.y = y * speed;
twist.linear.z = z * speed;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = th * turn;
// Publish it and resolve any remaining callbacks
pub.publish(twist);
ros::spinOnce();
}
return 0;
}
다음은 수정한 코드이다.
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <map>
// Map for movement keys
std::map<char, std::vector<float>> moveBindings
{
{'w', { 1, 0, 0}},
{'a', { 0, -1, 0}},
{'d', { 0, 1, 0}},
{'s', {-1, 0, 0}}
};
// Map for speed keys
std::map<char, std::vector<float>> speedBindings
{
{'i', { 0.5, 0}},
{'o', {-0.5, 0}},
{'k', { 0, 1.0}},
{'l', { 0, -1.0}}
};
// Reminder message
const char* msg = R"(
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
w
a s d
x
velocity
i : up (+0.5) m/s
o : down (-0.5) m/s
turn weight
k : up (+1,0) rad/s
l : down (-1,0) rad/s
anything else : stop
CTRL-C to quit
)";
// Init variables
float velocity(0.5); // Linear velocity (m/s)
float turn(1.0); // Angular velocity (rad/s)
float x(0), y(0), z(0), th(0); // Forward/backward/neutral direction vars
char key(' ');
// For non-blocking keyboard inputs
int getch(void)
{
int ch;
struct termios oldt;
struct termios newt;
// Store old settings, and copy to new settings
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
// Make required changes and apply the settings
newt.c_lflag &= ~(ICANON | ECHO);
newt.c_iflag |= IGNBRK;
newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
newt.c_cc[VMIN] = 1;
newt.c_cc[VTIME] = 0;
tcsetattr(fileno(stdin), TCSANOW, &newt);
// Get the current character
ch = getchar();
// Reapply old settings
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
int main(int argc, char** argv)
{
// Init ROS node
ros::init(argc, argv, "teleop_twist_keyboard");
ros::NodeHandle nh;
// Init cmd_vel publisher
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
// Create Twist message
geometry_msgs::Twist twist;
printf("%s", msg);
printf("\rCurrent: velocity %f\tturn %f | Awaiting command...\r", velocity, turn);
while(true)
{
// Get the pressed key
key = getch();
// If the key corresponds to a key in moveBindings
if (moveBindings.count(key) == 1)
{
// Grab the direction data
x = moveBindings[key][0];
y = moveBindings[key][1];
z = moveBindings[key][2];
}
// Otherwise if it corresponds to a key in speedBindings
else if (speedBindings.count(key) == 1)
{
// Grab the speed data
velocity = velocity + speedBindings[key][0];
turn = turn + speedBindings[key][1];
}
// Otherwise, set the robot to stop
else
{
velocity = 0;
turn = 0;
// If ctrl-C (^C) was pressed, terminate the program
if (key == '\x03')
break;
}
printf("\rCurrent: velocity %f m/s\tturn %f rad/s\tLast command: %c ", velocity, turn, key);
// Update the Twist message
twist.linear.x = x;
twist.linear.y = y;
twist.linear.z = 0;
twist.angular.x = velocity;
twist.angular.y = turn;
twist.angular.z = 0;
// Publish it and resolve any remaining callbacks
pub.publish(twist);
ros::spinOnce();
}
return 0;
}수정이후 catkin_make 해준다.
7. Subscriber 수정
6개의 값중 4개의 값만 사용하므로 코드를 수정해 준다.
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
void response(const geometry_msgs::Twist::ConstPtr& msg)
{
ROS_INFO("x = %f",msg->linear.x);
ROS_INFO("y = %f",msg->linear.y);
ROS_INFO("speed = %f",msg->angular.x);
ROS_INFO("turn = %f",msg->angular.y);
ROS_INFO("=======================");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "key_sub");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("cmd_vel",100,response);
ros::spin();
return 0;
}수정이후 catkin_make 해준다.
8. 수정된 코드 동작 확인
위와 동일하게 3개의 터미널을 이용해 동작을 확인한다.
#1st Terminal
$ roscore
#2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard
#3rd Terminal
$ rosrun teleop_twist_keyboard_cpp key_sub
9. OpenCR 이용해 모터 구동
- 9.1 IG-52GM+Encoder 04TYPE (24V)
6개 색의 선이 존재한다.
빨간색 : Motor VCC
검은색 : Motor GND
갈색 : Encoder VCC
초록색 : Encoder GND
파란색 : Encoder B
보라색 : Encoder A
- 9.2 L298N
2개의 모터를 동시에 제어할 수 있는 모터 드라이버이다.
모터에서 Motor VCC, GND는 드라이버 좌우에 하나씩 있는 핀에 연결하는데, 연결하는 방법에 따라 모터 회전 방향이 바뀐다.
최대 24V까지 입력이 가능하며, 정면 왼쪽에 있는 3개의 핀 중 VCC, GND에 각각 연결한다.
정면 오른쪽에 10개의 핀 중 ENA, IN1, IN2를 통해 소프트웨어적으로 모터의 회전방향을 결정한다. ENB, IN3, IN4를 통해 오른쪽에 연결된 모터를 제어할 수 있다.
- 9.3 연결
Ooen CR <-> MOTOR / DRIVER
| Open CR | 5V | GND | 3 | 4 | 8 | 9 | 10 |
| MOTOR | ENCODER VCC |
ENCODER GND |
ENCODER A | ENCODER B | |||
| DRIVER | IN2 | IN1 | ENA |
ENA 핀은 ~표시가 있는 PWM 핀에 연결해야 한다.
POWER SUPPLY <-> DRIVER <-> MOTOR
| POWER SUPPLY | VCC | GND | ||
| MOTOR | MOTOR VCC | MOTOR GND | ||
| DRIVER | VCC | GND | OUT 1 | OUT 2 |
- 9.4 코드 작성
아두이노에 맞는 코드를 작성한다. 테스트 용으로 작성하였다.
//ros
#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
//motor
#define ENABLE_A 10
#define IN1_A 9
#define IN2_A 8
ros::NodeHandle nh;
float velocity = 0;
void MOTOR( const geometry_msgs::Twist& msg)
{
if(msg.linear.x == 1) //w
{
digitalWrite(IN1_A, HIGH);
digitalWrite(IN2_A, LOW);
velocity = msg.angular.x;
}
else if(msg.linear.x == -1) //s
{
digitalWrite(IN1_A, LOW);
digitalWrite(IN2_A, HIGH);
velocity = msg.angular.x;
}
else if(msg.linear.y == -1) //a
{
digitalWrite(IN1_A, HIGH);
digitalWrite(IN2_A, LOW);
velocity = msg.angular.y;
}
else if(msg.linear.y == 1) //d
{
digitalWrite(IN1_A, LOW);
digitalWrite(IN2_A, HIGH);
velocity = msg.angular.y;
}
else if(msg.linear.x == msg.linear.y)
velocity = 0;
else;
analogWrite(ENABLE_A, velocity);
}
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &MOTOR);
void setup()
{
pinMode(IN1_A, OUTPUT); // 방향1
pinMode(IN2_A, OUTPUT); // 방향2
pinMode(ENABLE_A, OUTPUT); // 속도
nh.initNode();
nh.subscribe(sub);
}
void loop(){
nh.spinOnce();
delay(1);
}
이후 3개의 터미널에 각각의 명령어를 입력하여 실행한다.
#1st Terminal
$ roscore
#2nd Terminal
$ rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard
#3rd Terminal
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600
'한이음 > GPS 좌표 트래킹을 통한 자율주행 로봇' 카테고리의 다른 글
| ROS 삭제 (0) | 2022.04.25 |
|---|---|
| ROS OpenCR Motor 구동 / Encoder 값 받기 (0) | 2022.03.25 |
| OpenCR 사용하기 (0) | 2022.02.05 |
| IMU (Myahrs+) 사용하기 (0) | 2022.02.03 |
| ROS에서 Arduino로 값 전달 (ROS (PUB)-> Arduino (SUB)) (0) | 2022.02.02 |