From dc89d37c2206e287f27a75adf807ef71fa375cad Mon Sep 17 00:00:00 2001 From: isvogor Date: Thu, 16 Feb 2017 20:58:21 -0500 Subject: [PATCH] Changed message to CommandLong, works! Todo: landing --- include/roscontroller.h | 9 ++++--- launch/launch_config/solo.yaml | 3 ++- launch/rosbuzz.launch | 2 +- src/buzzuav_closures.cpp | 46 +++++++++++++++++----------------- src/roscontroller.cpp | 34 +++++++++++++++++-------- src/test1.bzz | 1 + 6 files changed, 56 insertions(+), 39 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index dc9d6aa..9ccbaf4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -4,6 +4,7 @@ #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandInt.h" +#include "mavros_msgs/CommandLong.h" #include "mavros_msgs/ExtendedState.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" @@ -64,8 +65,9 @@ private: ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; /*Commands for flight controller*/ - mavros_msgs::CommandInt cmd_srv; - + //mavros_msgs::CommandInt cmd_srv; + mavros_msgs::CommandLong cmd_srv; + std::vector m_sMySubscriptions; std::map m_smTopic_infos; @@ -126,8 +128,7 @@ private: void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); /* RC commands service */ - bool rc_callback(mavros_msgs::CommandInt::Request &req, - mavros_msgs::CommandInt::Response &res); + bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index f7e47c1..105f868 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -9,4 +9,5 @@ type: # for solo #battery : mavros_msgs/BatteryStatus status : mavros_msgs/State - +environment : + environment : solo-simulator diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 3424911..242cf44 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -7,7 +7,7 @@ - + diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1471ada..d48f689 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -171,40 +171,40 @@ int buzzuav_goto(buzzvm_t vm) { } /******************************/ -double* getgoto(){ -return goto_pos; +double* getgoto() { + return goto_pos; } /******************************/ -int getcmd(){ - return cur_cmd; +int getcmd() { + return cur_cmd; } -void set_goto(double pos[]){ -goto_pos[0]=pos[0]; -goto_pos[1]=pos[1]; -goto_pos[2]=pos[2]; - +void set_goto(double pos[]) { + goto_pos[0] = pos[0]; + goto_pos[1] = pos[1]; + goto_pos[2] = pos[2]; + } -int bzz_cmd(){ -int cmd = buzz_cmd; -buzz_cmd=0; -return cmd; +int bzz_cmd() { + int cmd = buzz_cmd; + buzz_cmd = 0; + return cmd; } -void rc_set_goto(double pos[]){ -rc_goto_pos[0]=pos[0]; -rc_goto_pos[1]=pos[1]; -rc_goto_pos[2]=pos[2]; - +void rc_set_goto(double pos[]) { + rc_goto_pos[0] = pos[0]; + rc_goto_pos[1] = pos[1]; + rc_goto_pos[2] = pos[2]; + } -void rc_call(int rc_cmd_in){ -rc_cmd=rc_cmd_in; +void rc_call(int rc_cmd_in) { + rc_cmd = rc_cmd_in; } -void set_obstacle_dist(float dist[]){ - for(int i=0; i<5;i++) - obst[i]=dist[i]; +void set_obstacle_dist(float dist[]) { + for (int i = 0; i < 5; i++) + obst[i] = dist[i]; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 12656f4..bd6b885 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -179,7 +179,15 @@ namespace rosbzz_node{ payload_pub = n_c.advertise(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); /* Clients*/ - mav_client = n_c.serviceClient(fcclient_name); + + //if(fcclient_name == "/mavros/cmd/command"){ + // mav_client = n_c.serviceClient(fcclient_name); + //} + //else { + //TODO: Here + mav_client = n_c.serviceClient(fcclient_name); + //} + multi_msg=true; } @@ -241,15 +249,16 @@ namespace rosbzz_node{ int tmp = buzzuav_closures::bzz_cmd(); double* goto_pos = buzzuav_closures::getgoto(); if (tmp == 1){ - cmd_srv.request.z = goto_pos[2]; + cmd_srv.request.param7 = goto_pos[2]; + //cmd_srv.request.z = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller"); } else if (tmp == 2) { /*FC call for goto*/ /*prepare the goto publish message */ - cmd_srv.request.x = goto_pos[0]*pow(10,7); - cmd_srv.request.y = goto_pos[1]*pow(10,7); - cmd_srv.request.z = goto_pos[2]; + cmd_srv.request.param5 = goto_pos[0]*pow(10,7); + cmd_srv.request.param6 = goto_pos[1]*pow(10,7); + cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller"); @@ -535,6 +544,7 @@ namespace rosbzz_node{ /*flight extended status update*/ void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){ // http://wiki.ros.org/mavros/CustomModes + // TODO: Handle landing std::cout << "Message: " << msg->mode << std::endl; if(msg->mode == "GUIDED") buzzuav_closures::flight_status_update(1); @@ -647,8 +657,8 @@ namespace rosbzz_node{ } /* RC command service */ - bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req, - mavros_msgs::CommandInt::Response &res){ + bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, + mavros_msgs::CommandLong::Response &res){ int rc_cmd; /* if(req.command==oldcmdID) return true; @@ -675,9 +685,13 @@ namespace rosbzz_node{ case mavros_msgs::CommandCode::NAV_WAYPOINT: ROS_INFO("RC_Call: GO TO!!!! "); double rc_goto[3]; - rc_goto[0]=req.x/pow(10,7); - rc_goto[1]=req.y/pow(10,7); - rc_goto[2]=req.z; + //rc_goto[0]=req.x/pow(10,7); + //rc_goto[1]=req.y/pow(10,7); + //rc_goto[2]=req.z; + rc_goto[0] = req.param5 / pow(10, 7); + rc_goto[1] = req.param6 / pow(10, 7); + rc_goto[2] = req.param7; + buzzuav_closures::rc_set_goto(rc_goto); rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); diff --git a/src/test1.bzz b/src/test1.bzz index 719e929..5298fe8 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -116,6 +116,7 @@ function takeoff() { } } function land() { + log("Land: ", flight.status) if( flight.status == 1) { barrier_set(ROBOTS,idle) barrier_ready()