Spiri
/
ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changed message to CommandLong, works! Todo: landing

This commit is contained in:
isvogor 2017-02-16 20:58:21 -05:00
parent 807602bd27
commit dc89d37c22
6 changed files with 56 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
include/roscontroller.h
View File

@ -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<std::string> m_sMySubscriptions;
std::map<std::string, std::string> 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);

3
launch/launch_config/solo.yaml
View File

@ -9,4 +9,5 @@ type:
# for solo
#battery : mavros_msgs/BatteryStatus
status : mavros_msgs/State
environment :
environment : solo-simulator

2
launch/rosbuzz.launch
View File

@ -7,7 +7,7 @@
<param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" />
<!-- todo: verify if this will work, or use "command" -->
<param name="fcclient_name" value="/mavros/cmd/command_int" />
<param name="fcclient_name" value="/mavros/cmd/command" />
<param name="in_payload" value="/inMavlink"/>
<param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/>

46
src/buzzuav_closures.cpp
View File

@ -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];
}

34
src/roscontroller.cpp
View File

@ -179,7 +179,15 @@ namespace rosbzz_node{
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
/* Clients*/
mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
//if(fcclient_name == "/mavros/cmd/command"){
// mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
//}
//else {
//TODO: Here
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(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);

1
src/test1.bzz
View File

@ -116,6 +116,7 @@ function takeoff() {
}
}
function land() {
log("Land: ", flight.status)
if( flight.status == 1) {
barrier_set(ROBOTS,idle)
barrier_ready()