[pixhawk] gazebo 환경에서 mavros를 이용해 위치명령주기

직접 노드 통신을 통해 메시지를 주는방법도 있을 것 같은데, 일단 예제소스가 있어 활용해보았다.

 

cd ~/catkin_ws/src
git clone https://github.com/Jaeyoung-Lim/modudculab_ros.git
cd ..
catkin_make

roscore
-----------------------------
make px4_sitl_default gazebo
-----------------------------
roslaunch modudculab_ros ctrl_pos_gazebo.launch
----------------------------------------------- 각각 다른 터미널창에서 실행
rosrun mavros mavsafety arm
rosrun mavros mavsys mode -c OFFBOARD

1미터 상공에서 호버링 하는 모습을 볼 수 있다. modudculab/src/pub_setpoints_pos.cpp을 수정하면 원하는 위치로 조정가능하다.

간단하게 반복 이동하면서 현재 위치를 출력하게끔 수정해보았다.

 

위치를 받아오기위해서는 PoseStamped에 mavros/local_position/pose 토픽을 subcribe 해야했다. 일일히 뜯으면서 하려니 시간도 오래걸리고..

 

#include <ros/ros.h>
#include <std_msgs/String.h> 
#include <stdio.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
geometry_msgs::PoseStamped current_pose;
void state_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    current_pose = *msg;
    ROS_INFO("Received(x,y,z): %4.2f, %4.2f, %4.2f\n",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
}
int main(int argc, char **argv)
{
   ros::init(argc, argv, "pub_setpoints");
   ros::NodeHandle n;

   ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local",10);
   ros::Subscriber chatter_sub = n.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose",10,state_cb);
   ros::Rate loop_rate(100);
   ros::spinOnce();

   geometry_msgs::PoseStamped msg;
   int count = 1;
	int plus = 1;
		//PositionReciever qp;:
		//Body some_object;
		//qp.connect_to_server();

	
   while(ros::ok()){
	   //some_object = qp.getStatus();
		// some_object.print();
		//printf("%f\n",some_object.position_x);
       msg.header.stamp = ros::Time::now();
       msg.header.seq=count;
       msg.header.frame_id = 1;
       if (current_pose.pose.position.x>20)
      {
         plus=1;
      }
      
       msg.pose.position.x = 1.0+plus;//0.001*some_object.position_x;
       msg.pose.position.y = 2.0+plus;//0.001*some_object.position_y;
       msg.pose.position.z = 5.0;//0.001*some_object.position_z;
       //msg.pose.orientation.x = 0;
       //msg.pose.orientation.y = 0;
       //msg.pose.orientation.z = 0;
       //msg.pose.orientation.w = 1;

       chatter_pub.publish(msg);
       ros::spinOnce();
       count++;
       plus++;
       loop_rate.sleep();
   }
   
      
   return 0;
}

 

A* 알고리즘 이용해서 x,y 경로계획까지 해봐야겠당

 

 

 

 

 

log file 기록

mkdir ~/logfiles

cd logfiles/

 

rostopic list -v //topic 확인

rosbag record -O logfilename topic

 

일정시간 후 

rosbag info iris_default_1.bag

rqt_bag 으로 열어서 저장한 로그파일 열기

(publish 체크하기)