1. 패키지 생성
– 패키지 이름 : zetabank_robot_control
– zetabank_diffwheeled_robot_control.cpp : motor driver와 serial 통신, twist의 cmd_vel을 전달받아 모터 드라이버 제어(시리얼 통신), 현재 이동거리, 속도, 센서 데이터 퍼블리쉬
catkin_create_pkg diffwheel_robot_control roscpp rospy serial std_msg geometry_msg
2. 컴파일 환경 구축
먼저 패키지를 생성한 “zetabank_robot_control” 폴더로 이동합니다.
여기서 package.xml 파일과 다음과 같이 수정합니다.
<?xml version=”1.0″?> <package format=”2″> <name>diffwheel_robot_control</name> <version>0.1.0</version> <description>The differential wheel robot control package</description> <author email=”sjyong@humanoidsystem.com”>Seo, Jae Yong</author> <maintainer email=”sjyong@humanoidsystem.com”>Seo, Jae Yong</maintainer> <license>BSD</license> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>roscpp</build_depend> <build_depend>rospy</build_depend> <build_depend>serial</build_depend> <build_depend>std_msgs</build_depend> <build_export_depend>geometry_msgs</build_export_depend> <build_export_depend>roscpp</build_export_depend> <build_export_depend>rospy</build_export_depend> <build_export_depend>serial</build_export_depend> <build_export_depend>std_msgs</build_export_depend> <exec_depend>geometry_msgs</exec_depend> <exec_depend>roscpp</exec_depend> <exec_depend>rospy</exec_depend> <exec_depend>serial</exec_depend> <exec_depend>std_msgs</exec_depend> </package>
CMakeLists.txt 파일을 아래의 내용으로 수정합니다.
cmake_minimum_required(VERSION 2.8.3)
project(diffwheel_robot_control)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
serial
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
geometry_msgs
roscpp
rospy
serial
std_msgs
tf
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(${PROJECT_NAME}_node src/diffwheel_robot_control.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
#############
## Install ##
#############
## Mark executables and/or libraries for installation
install(TARGETS diffwheel_robot_control
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark all other useful stuff for installation
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
diffwheel_robot_control_node.cpp 파일을 아래와 같이 작성합니다.
#include <ros/ros.h>
#include <ros/time.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>
#define LEFT 0
#define RIGHT 1
#define LINEAR 0
#define ANGULAR 1
#define MAX_LINEAR_VELOCITY 2.0 // m/s (BURGER : 0.22, WAFFLE : 0.25)
#define MAX_ANGULAR_VELOCITY 2.0 // rad/s (BURGER : 2.84, WAFFLE : 1.82)
#define LINEAR_X_MAX_VELOCITY 2.0
#define PI 3.1415926535897932384626433832795
#define MATH_RAD2DEG 57.2957795f
#define MATH_DEG2RAD 0.0174532f
#define WHEEL_NUM 2
#define WHEEL_RADIUS 0.0812 // meter
#define WHEEL_SEPARATION 0.360 // meter (BURGER : 0.160, WAFFLE : 0.287, zetabank : 0.360)
#define GEARRATIO 26
#define ENCODER_MIN -2147483648 // raw
#define ENCODER_MAX 2147483648 // raw
#define VELOCITY_UNIT 2
#define DISTORPM (60.0*GEARRATIO)/(2*PI*WHEEL_RADIUS)
#define CONTROL_MOTOR_SPEED_PERIOD 1000/50 //50hz. 20ms
float goal_velocity[VELOCITY_UNIT] = {0.0, 0.0};
float goal_velocity_from_cmd[VELOCITY_UNIT] = {0.0, 0.0};
bool teleop_flg = false;
float pre_velocity[VELOCITY_UNIT] = {0.0, 0.0};
unsigned int vel_same_cnt = 0;
unsigned int vel_total_same_cnt = 0;
uint32_t tTime[5];
ros::Time current_time;
uint64_t current_offset;
float prev_wheel_velocity_cmd[2];
std_msgs::String motorctrl_str;
void write_callback(const std_msgs::String::ConstPtr& msg);
// Callback function prototypes
void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg);
float constrain(float value, float min, float max);
void MotorDriver_Enable();
void MotorDriver_Disable();
void MotorDriver_SetMode();
void MotorDriver_SetSpeed(int leftSpeed, int rightSpeed);
void init_MotorController();
bool controlMotor(float * value);
void updateGoalVelocity(void);
void check_vel_safety(float *velocity);
ros::Time rosNow();
serial::Serial serial_comm;
int main (int argc, char** argv){
ros::init(argc, argv, "zetabank_robot_control_node");
ros::NodeHandle nh;
//nh.iniNode();
ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);
ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
ros::NodeHandle node_obj;
ros::Subscriber cmdvel_subscriber = node_obj.subscribe("cmd_vel",10,commandVelocityCallback);
//ros::spin();
try
{
serial_comm.setPort("/dev/ttyUSB0");
serial_comm.setBaudrate(9600);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
serial_comm.setTimeout(to);
serial_comm.open();
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
if(serial_comm.isOpen()){
ROS_INFO_STREAM("Serial Port initialized");
std_msgs::String test_string;
test_string.data = "Send test string!!!";
serial_comm.write(test_string.data);
} else {
return -1;
}
ros::Rate loop_rate(10);
uint32_t t;
tTime[0] = 0;
prev_wheel_velocity_cmd[0] = 0.0f;
prev_wheel_velocity_cmd[1] = 0.0f;
init_MotorController();
ROS_INFO_STREAM("Ready....");
while(ros::ok()){
t = ros::Time::now().toNSec();
current_offset = ros::Time::now().toNSec();
current_time = ros::Time::now();
if((t- tTime[0]) >= (CONTROL_MOTOR_SPEED_PERIOD)) {
updateGoalVelocity();
//check_vel_safety(goal_velocity);
controlMotor(goal_velocity);
}
ros::spinOnce();
if(serial_comm.available()){
ROS_INFO_STREAM("Reading from serial port");
std_msgs::String result;
result.data = serial_comm.read(serial_comm.available());
ROS_INFO_STREAM("Read: " << result.data);
read_pub.publish(result);
}
loop_rate.sleep();
}
MotorDriver_Disable();
}
void write_callback(const std_msgs::String::ConstPtr& msg){
ROS_INFO_STREAM("Writing to serial port" << msg->data);
serial_comm.write(msg->data);
}
float constrain(float value, float min, float max)
{
if(value > max) return max;
if(value < min) return min;
return value;
}
/*******************************************************************************
Callback function for cmd_vel msg
*******************************************************************************/
void commandVelocityCallback(const geometry_msgs::Twist & cmd_vel_msg)
{
goal_velocity_from_cmd[LINEAR] = cmd_vel_msg.linear.x;
goal_velocity_from_cmd[ANGULAR] = cmd_vel_msg.angular.z;
goal_velocity_from_cmd[LINEAR] = constrain(
goal_velocity_from_cmd[LINEAR],
(-1) * MAX_LINEAR_VELOCITY,
MAX_LINEAR_VELOCITY
);
goal_velocity_from_cmd[ANGULAR] = constrain(
goal_velocity_from_cmd[ANGULAR],
(-1) * MAX_ANGULAR_VELOCITY,
MAX_ANGULAR_VELOCITY
);
}
void MotorDriver_Enable()
{
motorctrl_str.data = "PE0001;";
if(serial_comm.isOpen()){
serial_comm.write(motorctrl_str.data);
}
}
void MotorDriver_Disable()
{
motorctrl_str.data = "PD0001;";
if(serial_comm.isOpen()){
serial_comm.write(motorctrl_str.data);
}
}
void MotorDriver_SetMode()
{
motorctrl_str.data = "SM0505;";
if(serial_comm.isOpen()){
serial_comm.write(motorctrl_str.data);
}
}
void MotorDriver_SetSpeed(int leftSpeed, int rightSpeed)
{
std::stringstream ss;
ss << "SV" << leftSpeed << "," << rightSpeed;
motorctrl_str.data = ss.str();
if(serial_comm.isOpen()){
serial_comm.write(motorctrl_str.data);
//if(leftspeed)
ROS_INFO_STREAM("Set Speed (" << leftSpeed << "," << rightSpeed << ")");
}
}
void init_MotorController()
{
ros::Rate idle(10000);
MotorDriver_Enable();
idle.sleep();
MotorDriver_SetMode();
idle.sleep();
ROS_INFO_STREAM("Initialize motor driver");
}
bool controlMotor(float * value)
{
float wheel_velocity_cmd[2];
wheel_velocity_cmd[LEFT] = value[LINEAR] - (value[ANGULAR] * WHEEL_SEPARATION / 2.0f);
wheel_velocity_cmd[RIGHT] = value[LINEAR] + (value[ANGULAR] * WHEEL_SEPARATION / 2.0f);
wheel_velocity_cmd[LEFT] = constrain(wheel_velocity_cmd[LEFT], -LINEAR_X_MAX_VELOCITY, LINEAR_X_MAX_VELOCITY);
wheel_velocity_cmd[RIGHT] = constrain(wheel_velocity_cmd[RIGHT], -LINEAR_X_MAX_VELOCITY, LINEAR_X_MAX_VELOCITY);
wheel_velocity_cmd[LEFT] = -1.0*wheel_velocity_cmd[LEFT]*DISTORPM;
wheel_velocity_cmd[RIGHT] = 1.0*wheel_velocity_cmd[RIGHT]*DISTORPM;
if((prev_wheel_velocity_cmd[LEFT] != wheel_velocity_cmd[LEFT]) || (prev_wheel_velocity_cmd[RIGHT] != wheel_velocity_cmd[RIGHT])) {
MotorDriver_SetSpeed((int)wheel_velocity_cmd[LEFT], (int)wheel_velocity_cmd[RIGHT]);
prev_wheel_velocity_cmd[LEFT] = wheel_velocity_cmd[LEFT];
prev_wheel_velocity_cmd[RIGHT] = wheel_velocity_cmd[RIGHT];
}
return true;
}
void updateGoalVelocity(void)
{
// Recieve goal velocity through ros messages
goal_velocity[LINEAR] = goal_velocity_from_cmd[LINEAR];
goal_velocity[ANGULAR] = goal_velocity_from_cmd[ANGULAR];
}
void check_vel_safety(float *velocity)
{
if(teleop_flg == true)
{
return ;
}
vel_total_same_cnt++;
if( (pre_velocity[LINEAR] == velocity[LINEAR]) && (pre_velocity[ANGULAR] == velocity[ANGULAR]) )
{
vel_same_cnt++;
}
if(vel_same_cnt >= 500 && vel_total_same_cnt >= 500)
{
char log_msg[10];
goal_velocity_from_cmd[LINEAR] = 0;
goal_velocity_from_cmd[ANGULAR] = 0;
MotorDriver_SetSpeed(0, 0);
// sprintf(log_msg, "Same_vel");
// nh.loginfo(log_msg);
vel_same_cnt = 0;
}
else if(vel_total_same_cnt >= 500)
{
//For clear the case of having same velocity
vel_same_cnt = 0;
vel_total_same_cnt = 0;
}
pre_velocity[LINEAR] = velocity[LINEAR];
pre_velocity[ANGULAR] = velocity[ANGULAR];
}
ros::Time rosNow()
{
uint32_t sec, nsec;
uint64_t _micros = ros::Time::now().toNSec() - current_offset;
sec = (uint32_t)(_micros / 1000000) + current_time.sec;
nsec = (uint32_t)(_micros % 1000000) + 1000 * (current_time.nsec / 1000);
if (nsec >= 1e9) {
sec++,
nsec--;
}
return ros::Time(sec, nsec);
}
3. 패키지를 컴파일 합니다.
catkin_make diffwheel_robot_control diffwheel_robot_control_node
4. 패키지를 실행합니다.
rosrun diffwheel_robot_control diffwheel_robot_control_node
