poses vector globali droneların yerini tutuyor

commander.cpp

@@ -0,0 +1,308 @@
+#include <ros/ros.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/Image.h>
+
+#include <move/PositionCommand.h>
+#include <move/Battery.h>
+#include <move/Position.h>
+#include <move/Camera.h>
+#include<vector>
+
+
+// Holds the given command by the services provided
+// Then sends the commands to drone in the main function
+geometry_msgs::TwistStamped pose_command;
+
+
+
+// Holds the pose of the drone
+
+
+std::vector<geometry_msgs::PoseStamped> poses;
+
+void setPoses(){
+    for(int i=0;i<10;i++){
+        geometry_msgs::PoseStamped pose;
+        poses.push_back(pose);
+    }
+}
+
+bool service_get_position(
+    move::Position::Request &req,
+    move::Position::Response &res
+) {
+    //res.x = pose0.pose.position.x;
+    //res.y = pose0.pose.position.y;
+    //res.z = pose0.pose.position.z;
+
+    return true;
+}
+
+// When the service called,
+// the requested x,y,z and t is put to the pose_command.
+bool service_command_global_position(
+    move::PositionCommand::Request &req,
+    move::PositionCommand::Response &res
+) {
+    ROS_INFO_STREAM("Move service is called");
+/*
+    pose_command.pose.position.x = req.x;
+    pose_command.pose.position.y = req.y;
+    pose_command.pose.position.z = req.z;
+*/
+    return true;
+}
+
+// When the service called,
+// the requested x,y,z and t is added to the pose_command.
+bool service_command_relative_position(
+    move::PositionCommand::Request &req,
+    move::PositionCommand::Response &res
+) {
+    ROS_INFO_STREAM("Move service is called");
+/*
+    pose_command.pose.position.x += req.x;
+    pose_command.pose.position.y += req.y;
+    pose_command.pose.position.z += req.z;
+*/
+    return true;
+}
+
+// Holds the battery state.
+sensor_msgs::BatteryState battery;
+
+// Responses the voltage, current and remaining percentage of battery when the service called.
+bool service_get_battery(
+    move::Battery::Request &req,
+    move::Battery::Response &res
+) {
+    ROS_INFO_STREAM("Battery service is called");
+
+    res.voltage = battery.voltage;
+    res.current = battery.current;
+    res.remaining = battery.percentage;
+
+    return true;
+}
+
+// Holds the camera frame if there is a camera connected and running.
+sensor_msgs::Image frame;
+
+// True if camera is connected, false otherwise.
+bool camera_connected=false;
+
+// Responses the frame of the camera
+// In response, gives height and width of the frame,
+// frame step, the representation is big endian or not, and the actual data.
+bool service_get_camera_frame(
+    move::Camera::Request &req,
+    move::Camera::Response &res
+) {
+	if(!camera_connected){
+		ROS_ERROR_STREAM("You are using drone without a camera. Please use correct drone(3dr iris with fpv camera");
+    }
+	else{
+	res.height= frame.height;
+	res.width= frame.width;
+	res.step= frame.step;
+	res.is_bigendian= frame.is_bigendian ;
+    res.data= frame.data;
+	ROS_INFO_STREAM("Data size = step*rows"); //WTF is that really working?
+	ROS_INFO_STREAM("Encoding RGB8");
+    }	
+}
+
+// This holds the current state of the drone
+// Connected, the flight mode etc.
+mavros_msgs::State current_state;
+
+// Saves the state of the drone
+void state_tracker(const mavros_msgs::State::ConstPtr& _current_state){
+    current_state = *_current_state;
+}
+
+// Saves the battery state of the drone
+void battery_tracker(const sensor_msgs::BatteryState::ConstPtr& _battery){
+    battery = *_battery;
+}
+
+// Saves the frames of the FPV camera
+void frame_save(const sensor_msgs::Image &_frame){
+	frame = _frame;
+    camera_connected=true; // Are there any other way to detect if camera is running?
+}
+
+// Saves the pose of the drone
+void pose_tracker0(const geometry_msgs::PoseStamped::ConstPtr& _pose){
+    poses[0] = *_pose;
+}
+void pose_tracker1(const geometry_msgs::PoseStamped::ConstPtr& _pose){
+    poses[1] = *_pose;
+}
+void pose_tracker2(const geometry_msgs::PoseStamped::ConstPtr& _pose){
+    poses[2] = *_pose;
+}
+void pose_tracker3(const geometry_msgs::PoseStamped::ConstPtr& _pose){
+    poses[3] = *_pose;
+}
+void pose_tracker4(const geometry_msgs::PoseStamped::ConstPtr& _pose){
+    poses[4] = *_pose;
+}
+
+
+
+int main(int argc, char **argv){
+
+    // Initializing node
+    // ros::init(argc, argv, "commander");
+    std::string uav_number_str = argv[1];
+    ROS_INFO_STREAM(uav_number_str);
+    int uav_number = std::stoi(uav_number_str);
+
+    int dumb = 0;
+    ros::init(dumb,NULL,"UAV" + uav_number_str);
+    ros::NodeHandle nh;
+
+    pose_command.twist.linear.x = 0;
+    pose_command.twist.linear.y = 0;
+    pose_command.twist.linear.z = 5;
+    pose_command.twist.angular.x = 0;
+    pose_command.twist.angular.y = 0;
+    pose_command.twist.angular.z = 0;
+
+    // Subscriber topics
+
+    // Current state of the drone
+    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
+            ("/uav" + uav_number_str +"/mavros/state", 10, state_tracker);
+    // FPV_cam connection
+	// ros::Subscriber camera_sub = nh.subscribe
+    //        ("/iris_fpv_cam/usb_cam/image_raw", 10, frame_save);
+    // Current battery of the drone
+    ros::Subscriber battery_status = nh.subscribe<sensor_msgs::BatteryState>
+            ("/uav" + uav_number_str +"/mavros/battery",10, battery_tracker);
+    // Current pose of the drone
+
+
+    setPoses();
+
+    ros::Subscriber position0 = nh.subscribe<geometry_msgs::PoseStamped>
+            ("/uav0/mavros/local_position/pose",10, pose_tracker0);
+    ros::Subscriber position1 = nh.subscribe<geometry_msgs::PoseStamped>
+            ("/uav1/mavros/local_position/pose",10, pose_tracker1);
+    ros::Subscriber position2 = nh.subscribe<geometry_msgs::PoseStamped>
+            ("/uav2/mavros/local_position/pose",10, pose_tracker2);
+    ros::Subscriber position3 = nh.subscribe<geometry_msgs::PoseStamped>
+            ("/uav3/mavros/local_position/pose",10, pose_tracker3);
+    ros::Subscriber position4 = nh.subscribe<geometry_msgs::PoseStamped>
+            ("/uav4/mavros/local_position/pose",10, pose_tracker4);
+
+    // Advertised topics
+
+    // Used for giving commands to drone.
+    ros::Publisher relative_position_command_publisher = nh.advertise<geometry_msgs::TwistStamped>
+            ("/uav" + uav_number_str +"/mavros/setpoint_velocity/cmd_vel", 10);
+
+    ROS_INFO_STREAM("/uav" + uav_number_str +"/mavros/setpoint_velocity/cmd_vel");
+    // Service Clients
+
+    // Changes arming status
+    ros::ServiceClient arm_command = nh.serviceClient<mavros_msgs::CommandBool>
+            ("/uav" + uav_number_str +"/mavros/cmd/arming");
+    // Set flight mode
+    ros::ServiceClient set_flight_mode = nh.serviceClient<mavros_msgs::SetMode>
+            ("/uav" + uav_number_str +"/mavros/set_mode");
+
+    // Service Servers
+    // These services are for external call.
+    // If you call these services from this file, you probably are doing something wrong.
+
+    ros::ServiceServer server_battery = nh.advertiseService(
+        "/uav" + uav_number_str + "/battery_status",
+        &service_get_battery
+    );
+    ros::ServiceServer server_camera_frame = nh.advertiseService(
+        "/uav" + uav_number_str + "/camera",
+        &service_get_camera_frame
+    );
+    ros::ServiceServer server_global_move_command = nh.advertiseService(
+        "/uav" + uav_number_str + "/position/command_global",
+         &service_command_global_position
+    );
+    ros::ServiceServer server_relative_move_command = nh.advertiseService(
+        "/uav" + uav_number_str + "/position/command_relative",
+         &service_command_relative_position
+    );
+    ros::ServiceServer server_position = nh.advertiseService(
+        "/uav" + uav_number_str + "/position/position",
+        &service_get_position
+    );
+
+    //the setpoint publishing rate MUST be faster than 2Hz
+    ros::Rate rate(20.0);
+
+    // wait for FCU connection
+    while(ros::ok() && !current_state.connected){
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+
+    ROS_INFO_STREAM("came here");
+
+
+    //send a few setpoints before starting
+    for(int i = 100; ros::ok() && i > 0; --i){ // The number of loops may be decreased probably.
+        relative_position_command_publisher.publish(pose_command);
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    // Used for setting the flight mode
+    mavros_msgs::SetMode set_mode;
+    set_mode.request.custom_mode = "OFFBOARD";
+
+    // Used for sending arm command 
+    mavros_msgs::CommandBool arm;
+    arm.request.value = true;
+
+    ros::Time last_request_time = ros::Time::now();
+    while(ros::ok()){
+        ROS_INFO_STREAM("in loop");
+        ROS_INFO_STREAM(current_state.mode);
+        ROS_INFO_STREAM(current_state.armed);
+        if( (current_state.mode != "AUTO.LOITER") && (ros::Time::now()-last_request_time > ros::Duration(5.0)) ){
+            if( set_flight_mode.call(set_mode) && set_mode.response.mode_sent){
+                ROS_INFO_STREAM("Offboard enabled");
+            }
+            last_request_time = ros::Time::now();
+        }
+        else{
+            if( !current_state.armed && (ros::Time::now() - last_request_time > ros::Duration(5.0)) ){
+                if( arm_command.call(arm) && arm.response.success){
+                    ROS_INFO_STREAM("Vehicle armed");
+                }
+                last_request_time = ros::Time::now();
+            }
+        }
+
+        //Here we are constantly publishing the position.
+		relative_position_command_publisher.publish(pose_command);
+        ROS_INFO_STREAM("pose 0 ");
+        ROS_INFO_STREAM(poses[0].pose.position.z);
+        ROS_INFO_STREAM("pose 1 ");
+        ROS_INFO_STREAM(poses[1].pose.position.z);
+
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    return 0;
+}