pro_control.launch파일 ( UARM로봇 기동하는 역할 )

pro_control.launch 파일은 

<launch>
	<param name="robot_description" command="cat $(find swiftpro)/urdf/pro_model.xacro" />
	<param name="use_gui" value= "False" />

	<node name="swiftpro_write_node" pkg="swiftpro" type="swiftpro_write_node"/>
	<node name="swiftpro_moveit_node" pkg="swiftpro" type="swiftpro_moveit_node"/>
	<node name="swiftpro_rviz_node" pkg="swiftpro" type="swiftpro_rviz_node"/>
	
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

이렇게 parameter 2개 node 4개를 실행하는데 UARM 을 움직이게 하는 핵심 XX다

---

roscore와 pro_control.launch 실행후 rqt_graph 로 찍어본 구성도 

---

 

---

1. swiftpro_moveit_node.cpp

처음 좌표를 받아오는 노드

	ros::Subscriber sub = n.subscribe("move_group/fake_controller_joint_states", 1, joint_Callback);
	ros::Publisher 	pub = n.advertise<swiftpro::position>("position_write_topic", 1);
	ros::Rate loop_rate(20);
	
	while (ros::ok())
	{
		swift_fk(motor_angle, position);
//swift_fk(motor_angle, position); 모토앵글에서 data받아서 로봇에 맞게 가공해서 postion으로 리턴 해줌
//위에 joint_Callback 함수가 call안되면 전역선언된 (60,60,60)data를 가공해서 pos.xyz에 넣어줌
		pos.x = position[0];
		pos.y = position[1];
		pos.z = position[2];
		printf("HOME 포지션 발행 (60,60,60)");
		pub.publish(pos);
		

	  
		ros::spinOnce();
		loop_rate.sleep();
	}

2. swiftpro_write_node.cpp

받아온 좌표를 가공(Gcode로)해서 로봇과 rviz에 "joint_state"를 publish 하는 노드

    Gcode로 넘겨주는 거로 로봇움직이고

    publish 한 "joint_state"로 Rviz로봇 움직인다.

 

  //callback 함수 호출되면 1. gcode로 로봇한테 data보내고 2. pos.@@@에 data 전부 저장해서 publish한다..(xyz값,그리퍼status체크,앵글값,그리퍼,펌프)
	ros::Subscriber sub1 = nh.subscribe("position_write_topic", 1, position_write_callback);
	ros::Subscriber sub2 = nh.subscribe("swiftpro_status_topic", 1, swiftpro_status_callback);
	ros::Subscriber sub3 = nh.subscribe("angle4th_topic", 1, angle4th_callback);
	ros::Subscriber sub4 = nh.subscribe("gripper_topic", 1, gripper_callback);
	ros::Subscriber sub5 = nh.subscribe("pump_topic", 1, pump_callback);
	ros::Publisher 	 pub = nh.advertise<swiftpro::SwiftproState>("SwiftproState_topic", 1);
	ros::Rate loop_rate(20);

	try
	{
		_serial.setPort("/dev/ttyACM0");
		_serial.setBaudrate(115200);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		_serial.setTimeout(to);
		_serial.open();
		ROS_INFO_STREAM("Port has been open successfully");
	}
	catch (serial::IOException& e)
	{
		ROS_ERROR_STREAM("Unable to open port");
		return -1;
	}
	
	if (_serial.isOpen())
	{
		ros::Duration(3.5).sleep();				// wait 3.5s
		_serial.write("M2120 V0\r\n");			// stop report position
		ros::Duration(0.1).sleep();				// wait 0.1s
		_serial.write("M17\r\n");				// attach
		ros::Duration(0.1).sleep();				// wait 0.1s
		ROS_INFO_STREAM("Attach and wait for commands");
	}

	while (ros::ok())
	{
		pub.publish(pos);
		ros::spinOnce();
		loop_rate.sleep();
	}

3. swiftpro_rviz_node.cpp

받은 좌표를 joint_angle[9] 로 가공해서 robot_state_publish로 쏴주는 노드,

broadcaster패키지로 odom_trans data를 tf 패키지로 보내줌.

    broadcaster : 이 클래스는 좌표 프레임 변환 정보를 publish 한다.

    odom_trans data : tf패키지에 publish하기위한 data type

    tf : tf 패키지로 rviz 노드의 로봇을 움직인다. (지금은 사용 X, tf2로 대체됬다)

---

broadcaster 랑 odom_trans 대한 참조

geometry_msgs::TransformStamped odom_trans;

http://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/TransformStamped.html 

geometry_msgs/TransformStamped Documentation

 

# 좌표 프레임 헤더.frame_id에서 변환을 나타냅니다.
# 좌표 프레임 child_frame_id에
#
# 이 메시지는 주로 다음 사용자가 사용합니다.
# TF 패키지.
# 자세한 내용은 설명서를 참조하십시오.

broadcaster 참조

http://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/TransformStamped.html

geometry_msgs/TransformStamped Documentation

 

---

ros::Subscriber sub = n.subscribe("SwiftproState_topic", 1, SwiftproState_Callback);
	ros::Publisher 	pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
	ros::Rate loop_rate(20);

	tf::TransformBroadcaster 		broadcaster;
	sensor_msgs::JointState 		joint_state;
	geometry_msgs::TransformStamped odom_trans;
	
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id  = "Base";
	
	while (ros::ok())
	{
		joint_state.header.stamp = ros::Time::now();
		joint_state.name.resize(9);
		joint_state.position.resize(9);
		joint_state.name[0] = "Joint1";
		joint_state.position[0] = joint_angle[0] / 57.2958;
	  joint_state.name[1] = "Joint2";
		joint_state.position[1] = joint_angle[1] / 57.2958;
		joint_state.name[2] = "Joint3";
		joint_state.position[2] = joint_angle[2] / 57.2958;
		joint_state.name[3] = "Joint4";
		joint_state.position[3] = joint_angle[3] / 57.2958;
		joint_state.name[4] = "Joint5";
		joint_state.position[4] = joint_angle[4] / 57.2958;
		joint_state.name[5] = "Joint6";
		joint_state.position[5] = joint_angle[5] / 57.2958;
		joint_state.name[6] = "Joint7";
		joint_state.position[6] = joint_angle[6] / 57.2958;
		joint_state.name[7] = "Joint8";
		joint_state.position[7] = joint_angle[7] / 57.2958;
		joint_state.name[8] = "Joint9";
		joint_state.position[8] = joint_angle[8] / 57.2958;

		odom_trans.header.stamp = ros::Time::now();
		odom_trans.transform.translation.x = 0;
		odom_trans.transform.translation.y = 0;
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(10);
		
		pub.publish(joint_state);
		broadcaster.sendTransform(odom_trans);
		
		ros::spinOnce();
		loop_rate.sleep();
	}

4. robot_state_publish

간편정리 : Parameter서버에 있는 param("robot_description")을 참조하고,

joint_angle[9] 정보가 들어간 joint_state를 subcribe해 -> 로봇state를 tf2패키지에 publish 해준다. (브로드캐스트 필없)

    "robot_description" <- control.launch 에서 init 함

    "joint_state" msg를 publish 하는 곳은 swiftpro_rviz_node.cpp

    tf2 패키지에 publish 하면 tf2를 모든 ros노드나 library에서 사용 할 수 있다. <- ex) Rviz에서 로봇 기동.

 

+알파 정리

패키지인데 /opt/ros/kinetic/lib/robot_state_publisher/ 폴더안에 있는 실행파일이다.

내용을 못봐서 ros위키 보고 정리함.

상세정리 :

1. paramerter server에 있는 <param name="robot_description" 에서 urdf 를 찾는다. (control.launch파일에서 init함)

2. 지정된 "joint_states" 네이밍을 가지는 Pulisher를 subcribe한다. ("swiftpro_rviz_node.cpp" 에서 Publisher함)

3_1. 이러한 정보를 가지고 로봇의 상태를 tf2 에 게시한다. 이 패키지는 라이브러리와 ROS 노드로 모두 사용할 수 있습니다.

3_2. tf2 란 : 사용자가 시간이 지남에 따라 여러 좌표 프레임을 추적할 수 있는 lib 임. (계속 갱신)

 

tf2는 사용자가 시간이 지남에 따라 여러 좌표 프레임을 추적할 수 있는 2세대 변환 라이브러리입니다. tf2는 시간에 버퍼링된 트리 구조에서 좌표 프레임 간의 관계를 유지하고 사용자가 원하는 시점에서 두 좌표 프레임 간의 점, 벡터 등을 변환할 수 있도록 합니다. 

 

참조한 정보 ( robot_state_publisher , tf2 , )

참조한정보

 

1. robot_state_publisher

http://wiki.ros.org/robot_state_publisher

robot_state_publisher - ROS Wiki

이 패키지를 사용하면 로봇의 상태를 tf2 에 게시할  수 있습니다 . 상태가 게시되면 tf2 를 사용하는 시스템의 모든 구성 요소에서 사용할 수 있습니다 . 패키지는 로봇의 관절 각도를 입력으로 받아 로봇의 운동학적 트리 모델을 사용하여 로봇 링크의 3D 포즈를 게시합니다. 패키지는 라이브러리와 ROS 노드로 모두 사용할 수 있습니다. 이 패키지는 잘 테스트되었으며 코드는 안정적입니다. 가까운 장래에 큰 변화가 계획되어 있지 않습니다.

robot_state_publisher는 로봇의 순운동학을 계산하고 tf 를 통해 결과를 게시하기 위해 매개변수 robot_description으로 지정된 URDF 와 joint_states 주제의 관절 위치를 사용합니다 .

자신의 로봇에서 로봇 상태 게시자를 사용하는 방법 에 대한 자습서를 참조하세요 . 로봇 상태 게시자를 사용하여 로봇 의 상태를 tf 에 게시하는 방법을 설명합니다

Subscribed topics

joint_states (sensor_msgs/JointState)

• joint position information

Parameters

robot_description (urdf map)

• The urdf xml robot description. This is accessed via `urdf_model::initParam`

tf_prefix (string)

• Set the tf prefix for namespace-aware publishing of transforms. See tf_prefix for more details.

publish_frequency (double)

• Publish frequency of state publisher, default: 50Hz.

ignore_timestamp (bool)

• If true, ignore the publish_frequency and the timestamp of joint_states and publish a tf for each of the received joint_states. Default is "false".

use_tf_static (bool)

• Set whether to use the /tf_static latched static transform broadcaster. Default: true.

 

2. tf2

https://wiki.ros.org/tf2   tf2 참조한 곳

http://library.isr.ist.utl.pt/docs/roswiki/robot_state_publisher(2f)Tutorials(2f)Using(20)the(20)robot(20)state(20)pub

lisher(20)on(20)your(20)own(20)robot.html

http://wiki.ros.org/tf2/Migration

tf -> tf2 바뀐점 차이

 

 

 

 

 

 

 

 

5. rosout

 

---