SungJaeShin / pose_movement

Pose Tracking

pose의 정보가 담겨있는 .csv file을 이용하여 rviz상으로 움직임 파악하기

csv file

[example]

• 498.00488,-436.80432,-4.7894874,0.55949587,-0.46982133,0.4388751,-0.52308786
  
• 차례대로 다음과 같은 값들을 의미한다.
  • position x = 498.00488
  • position y = -436.80432
  • position z = -4.7894874
  • orientation w = 0.55949587
  • orientation x = -0.46982133
  • orientation y = 0.4388751
  • orientation z = -0.52308786

Node & Topic explanation

[Node]

• "pose_movement" Node

[Topic]

• "pose" topic
  • message type : geometry_msgs::PoseStamped
    
• "tracking" topic
  • message type : nav_msgs::Path
    
• "odom" topic
  • message type : nav_msgs::Odometry
    

[Information]

• header.stamp ⇒ ros::Time::now()
• header.frame_id ⇒ base_link

Parameter explanation

• std::ifstream file ⇒ 나의 Computer에 있는 CSV file을 받을 변수
  
• std::vector result ⇒ CSV file을 comma(,) 제거 후 double type을 가진 std::vector로 넣어주기 위한 변수
  
• int index ⇒ 1set에 7개의 value들이 있기 때문에 구분하기 위해 설정
  
• double array[7] ⇒ 7개의 value들을 담기 위한 임시 배열이고 7개가 모두 채워진다면 이 값들을 publish하기 위해서 설정
  
• nav_msgs::Path path ⇒ pose의 이동경로를 출력하기 위해서 설정
  
• auto result_address ⇒ result의 첫 번째 주소값으로 움직이기 위해 설정
  
• geometry_msgs::PoseStamped pose_track ⇒ 현재 위치해있는 pose를 나타내기 위해 설정
  
• nav_msgs::Odometry odom ⇒ 시작점부터 끝날때까지의 경로를 출력하기 위해서 설정
  

overall code explanation

• function 1 ⇒ std::vector parseCSV(std::istream &file)
  • CSV file을 받아서 각 방들이 double type을 가지는 std::vector로 넣어준다.
  • 이 경우, comma(,)의 값을 빼고 Value들이 들어가도록 설정되어 있다.

• function 2 ⇒ geometry_msgs::PoseStamped get_pose(double x, double y, double z, double q_w, double q_x, double q_y, double q_z)
  • double type의 방을 가진 std::vector의 모든 값을 geometry_msgs::PoseStamped로 바꿔준다.
  • 이 경우, 한 줄씩 받아서 while문 안의 pose_track 에 넣어준다.