======ROS STUDY #6 : Topic 퍼블리시 실습======

{{youtube>7IAvfDXQsCk?medium}}

=====1. 만들어야 하는 패키지 분석=====

====1.1. CMD_VEL이란?====
{{:activity:staff:2021:rosstudy:turtlebot3_fake_node.png?600|}}

터틀봇의 속도 제어 명령을 내릴 때 사용하는 토픽이다. 
일반적으로 로봇의 이동을 제어하기 위해 많이 사용된다. 
형식은 geometry_msgs/Twist라는 형식을 사용한다.

====1.2. Twist====
<sxh bash>
linear.x
linear.y
linear.z
angular.x
angular.y
angular.z
</sxh>
Twist 메시지 형식은, Vector3 형식을 기반으로 한다. 
x,y,z축 방향으로의 선속도(linear), x,y,z축에 대한 각속도(angular) 데이터를 포함한다.

=====2. topic 패키지 생성=====
<sxh bash>
catkin_create_pkg cmd_vel_pub message_generation roscpp geometry_msgs message_runtime
catkin_create_pkg [만들 패키지 이름] [의존성 패키지1] [의존성 패키지2]...
</sxh>
catkin_ws로 이동 후, 터미널에서 위의 명령어 입력

====2.1. package.xml====
<sxh xml title:package.xml>
<package format="2">
    <name>cmd_vel_pub</name>
    <version>1.0.0</version>
    <description>cmd_vel_pub topic package</description>
    <maintainer email="TODO">TODO</maintainer>
    <license>TODO</license>

    <buildtool_depend>catkin</buildtool_depend>
    <build_depend>message_generation</build_depend>
    <build_depend>roscpp</build_depend>
    <build_depend>geometry_msgs</build_depend>
    <build_export_depend>roscpp</build_export_depend>
    <build_export_depend>geometry_msgs</build_export_depend>
    <exec_depend>roscpp</exec_depend>
    <exec_depend>geometry_msgs</exec_depend>
    <exec_depend>message_runtime</exec_depend>
    <export>
    </export>
</package>
</sxh>

====2.2. CMakeLists.txt====
작성한 노드를 빌트 할때 즉 소스코드 작성 후 실행파일을 만들때 사용되는 옵션들
<sxh cpp title:CMakeLists.txt>
cmake_minimum_required(VERSION 2.8.3)
project(cmd_vel_pub)           # 패키지 이름과 동일, 이름과 다르면 빌드가 안된다.

find_package(catkin REQUIRED COMPONENTS message_generation roscpp geometry_msgs)

include_directories(${catkin_INCLUDE_DIRS})

add_message_files(FILES Cmd_vel_msg.msg)           # 새로 만들 메세지 이름
generate_messages(DEPENDENCIES geometry_msgs)            # 의존성

##캐킨 패키지 옵션으로 라이브러리, 캐킨 빌드 위존성, 시스템 의존 패키지를 기술한다
catkin_package(
  INCLUDE_DIRS
  CATKIN_DEPENDS roscpp geometry_msgs
  DEPENDS
)

###########
## Build ##
###########
 
##실행파일
add_executable(cmd_vel_pub src/cmd_vel_pub.cpp)           # topic_publisher:노드, topic_publisher.cpp: 노드를 만들 때 참고해야할 소스코드
add_dependencies(cmd_vel_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(cmd_vel_pub ${catkin_LIBRARIES})
 
add_executable(sub_test src/sub_test.cpp)           # topic_subscriber:노드, topic_subscriber.cpp: 노드를 만들 때 참고해야할 소스코드
add_dependencies(sub_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(sub_test ${catkin_LIBRARIES})
</sxh>

=====3. 메세지(msg) 등록=====
<sxh cpp title:msg/Cmd_vel_msg.msg>
geometry_msgs/Twist cmd_vel
</sxh>


=====4. 노드 코드 작성=====

====4.1. 퍼블리셔(publisher)====
<sxh cpp title:src/cmd_vel_pub.cpp>
#include "ros/ros.h"
#include "cmd_vel_pub/Cmd_vel_msg.h"
 
#define PUB_NODE_NAME "cmd_vel" // name of node
#define SUB_NODE_NAME "cmd_vel_sub" // name of node
#define TOPIC_NAME "cmd_vel_topic" // name of topic : cmd_vel_pub

float vel_x, vel_y;
 
int main(int argc, char **argv){
  ros::init(argc, argv, PUB_NODE_NAME);
  ros::NodeHandle nh;
  geometry_msgs::Twist cmd_vel; // variable to publish
 
  ros::Publisher cmd_vel_publisher = nh.advertise<geometry_msgs::Twist>(TOPIC_NAME, 100, true);
  ros::Rate loop_rate(0.3);
 
    cmd_vel.linear.x =0;
    cmd_vel.linear.y =0;
 
  while (ros::ok()){
 
    std::cout << "input velocity" << std::endl;
    std::cin >> vel_x >> vel_y;
 
    float current_x = cmd_vel.linear.x;
    float current_y = cmd_vel.linear.y;
 

    loop_rate.sleep();// Goes to sleep according to the loop rate defined above.
 
  }
  return 0;
}
</sxh>

====4.2. 서브스크라이버(subscriber)====
<sxh cpp title:src/topic_subscriber.cpp>
#include "ros/ros.h"
#include "cmd_vel_pub/Cmd_vel_msg.h"
 
#define PUB_NODE_NAME "cmd_vel" // name of node
#define SUB_NODE_NAME "cmd_vel_sub" // name of node
#define TOPIC_NAME "cmd_vel_topic" // name of topic : cmd_vel_pub
 
void messageCb(const geometry_msgs::Twist& cmd_vel){
    ROS_INFO("linear.x : %f\n", cmd_vel.linear.x);
    ROS_INFO("linear.y : %f\n", cmd_vel.linear.y);
    ROS_INFO("linear.z : %f\n", cmd_vel.linear.z);
    ROS_INFO("angular.x : %f\n", cmd_vel.angular.x);
    ROS_INFO("angular.y : %f\n", cmd_vel.angular.y);
    ROS_INFO("angular.z : %f\n", cmd_vel.angular.z);
}
 
int main(int argc, char **argv){
    ros::init(argc, argv, SUB_NODE_NAME);
    ros::NodeHandle nh;
 
    // ros::Subscriber cmd_vel_subscriber(TOPIC_NAME, messageCb);
    ros::Subscriber cmd_vel_subscriber =nh.subscribe(TOPIC_NAME, 10, messageCb);
     
    ros::spin();
 
    return 0;
}
</sxh>