merged with dev, tested, adapted for solo
This commit is contained in:
isvogor
parent
0cd59a91f0
commit
91a398c02b
|
|
@ -74,7 +74,6 @@ private:
|
|||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Publisher local_position_pub;
|
||||
|
||||
/*Commands for flight controller*/
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
|
|
@ -94,8 +93,6 @@ private:
|
|||
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
|
||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle n_c);
|
||||
|
||||
|
|
@ -159,16 +156,16 @@ private:
|
|||
void Arm();
|
||||
|
||||
/*set mode like guided for solo*/
|
||||
void SetMode();
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle n_c);
|
||||
|
||||
void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
void fc_command_setup();
|
||||
|
||||
void LocalPosition(float x, float y, float z, float yaw);
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -78,27 +78,7 @@ namespace buzz_utility{
|
|||
size_t tot = sizeof(uint32_t);
|
||||
/* Go through the messages until there's nothing else to read */
|
||||
uint16_t unMsgSize=0;
|
||||
<<<<<<< HEAD
|
||||
//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
|
||||
//tot+=sizeof(uint8_t);
|
||||
//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
|
||||
//if(is_msg_pres){
|
||||
//unMsgSize = *(uint16_t*)(pl + tot);
|
||||
//tot+=sizeof(uint16_t);
|
||||
// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
|
||||
// if(unMsgSize>0){
|
||||
// code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
|
||||
// tot+=unMsgSize;
|
||||
//fprintf(stdout,"before in queue process : utils\n");
|
||||
// code_message_inqueue_process();
|
||||
//fprintf(stdout,"after in queue process : utils\n");
|
||||
// }
|
||||
//}
|
||||
//unMsgSize=0;
|
||||
|
||||
=======
|
||||
|
||||
>>>>>>> dev
|
||||
/*Obtain Buzz messages only when they are present*/
|
||||
do {
|
||||
/* Get payload size */
|
||||
|
|
@ -128,33 +108,6 @@ namespace buzz_utility{
|
|||
/* Send robot id */
|
||||
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
|
||||
tot += sizeof(uint16_t);
|
||||
<<<<<<< HEAD
|
||||
//uint8_t updater_msg_pre = 0;
|
||||
//uint16_t updater_msgSize= 0;
|
||||
//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
|
||||
// fprintf(stdout,"transfer code \n");
|
||||
// updater_msg_pre =1;
|
||||
//transfer_code=0;
|
||||
// *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
|
||||
// tot += sizeof(uint8_t);
|
||||
/*Append updater msg size*/
|
||||
//*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
|
||||
// updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
|
||||
// *(uint16_t*)(buff_send + tot)=updater_msgSize;
|
||||
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
|
||||
// tot += sizeof(uint16_t);
|
||||
/*Append updater msgs*/
|
||||
// memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
|
||||
// tot += updater_msgSize;
|
||||
/*destroy the updater out msg queue*/
|
||||
// destroy_out_msg_queue();
|
||||
//}
|
||||
//else{
|
||||
// *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
|
||||
// tot += sizeof(uint8_t);
|
||||
//}
|
||||
=======
|
||||
>>>>>>> dev
|
||||
/* Send messages from FIFO */
|
||||
do {
|
||||
/* Are there more messages? */
|
||||
|
|
@ -532,67 +485,67 @@ namespace buzz_utility{
|
|||
buzzvm_gstore(VM);*/
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*Destroy the bvm and other resorces*/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/*Destroy the bvm and other resorces*/
|
||||
/****************************************/
|
||||
|
||||
void buzz_script_destroy() {
|
||||
if(VM) {
|
||||
if(VM->state != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
}
|
||||
buzzvm_function_call(VM, "destroy", 0);
|
||||
buzzvm_destroy(&VM);
|
||||
free(BO_FNAME);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
}
|
||||
fprintf(stdout, "Script execution stopped.\n");
|
||||
}
|
||||
void buzz_script_destroy() {
|
||||
if(VM) {
|
||||
if(VM->state != BUZZVM_STATE_READY) {
|
||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||
BO_FNAME,
|
||||
buzz_error_info());
|
||||
buzzvm_dump(VM);
|
||||
}
|
||||
buzzvm_function_call(VM, "destroy", 0);
|
||||
buzzvm_destroy(&VM);
|
||||
free(BO_FNAME);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
}
|
||||
fprintf(stdout, "Script execution stopped.\n");
|
||||
}
|
||||
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
/****************************************/
|
||||
/*Execution completed*/
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
/*Execution completed*/
|
||||
/****************************************/
|
||||
|
||||
int buzz_script_done() {
|
||||
return VM->state != BUZZVM_STATE_READY;
|
||||
}
|
||||
int buzz_script_done() {
|
||||
return VM->state != BUZZVM_STATE_READY;
|
||||
}
|
||||
|
||||
int update_step_test(){
|
||||
int update_step_test() {
|
||||
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||
|
||||
int a = buzzvm_function_call(VM, "step", 0);
|
||||
if(a != BUZZVM_STATE_READY){
|
||||
fprintf(stdout, "step test VM state %i\n",a);
|
||||
fprintf(stdout, " execution terminated abnormally\n\n");
|
||||
}
|
||||
return a == BUZZVM_STATE_READY;
|
||||
}
|
||||
int a = buzzvm_function_call(VM, "step", 0);
|
||||
if(a != BUZZVM_STATE_READY) {
|
||||
fprintf(stdout, "step test VM state %i\n",a);
|
||||
fprintf(stdout, " execution terminated abnormally\n\n");
|
||||
}
|
||||
return a == BUZZVM_STATE_READY;
|
||||
}
|
||||
|
||||
uint16_t get_robotid(){
|
||||
/* Get hostname */
|
||||
char hstnm[30];
|
||||
gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm + 4, NULL, 10);
|
||||
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
|
||||
return (uint16_t)id;
|
||||
}
|
||||
uint16_t get_robotid() {
|
||||
/* Get hostname */
|
||||
char hstnm[30];
|
||||
gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm + 4, NULL, 10);
|
||||
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
|
||||
return (uint16_t)id;
|
||||
}
|
||||
|
||||
buzzvm_t get_vm(){
|
||||
return VM;
|
||||
}
|
||||
buzzvm_t get_vm() {
|
||||
return VM;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -161,14 +161,9 @@ namespace rosbzz_node{
|
|||
Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
|
||||
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
/*publishers*/
|
||||
|
||||
//mavros_msgs/PositionTarget
|
||||
local_position_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
|
||||
//mavros_msgs::PositionTarget message;
|
||||
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||
localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local",1000);
|
||||
localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local",1000);
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
|
|
@ -255,6 +250,7 @@ namespace rosbzz_node{
|
|||
void roscontroller::Arm(){
|
||||
mavros_msgs::CommandBool arming_message;
|
||||
arming_message.request.value = armstate;
|
||||
ROS_INFO("FC Arm Service called!------------------------------------------------------");
|
||||
if(arm_client.call(arming_message)) {
|
||||
if(arming_message.response.success==1)
|
||||
ROS_INFO("FC Arm Service called!");
|
||||
|
|
@ -264,20 +260,6 @@ namespace rosbzz_node{
|
|||
ROS_INFO("FC Arm Service call failed!");
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------
|
||||
/ Set mode for the solos
|
||||
/---------------------------------------------------------*/
|
||||
void roscontroller::SetMode(){
|
||||
mavros_msgs::SetMode set_mode_message;
|
||||
set_mode_message.request.base_mode = 0;
|
||||
set_mode_message.request.custom_mode = "GUIDED"; // shit!
|
||||
if(mode_client.call(set_mode_message)) {
|
||||
ROS_INFO("Set Mode Service call successful!");
|
||||
} else {
|
||||
ROS_INFO("Set Mode Service call failed!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*-----------------------------------------------------------------------------------------------------
|
||||
/Prepare Buzz messages payload for each step and publish
|
||||
|
|
@ -357,45 +339,59 @@ namespace rosbzz_node{
|
|||
/*---------------------------------------------------------------------------------
|
||||
/Flight controller service call every step if there is a command set from bzz script
|
||||
/-------------------------------------------------------------------------------- */
|
||||
void roscontroller::flight_controler_service_call(){
|
||||
void roscontroller::flight_controler_service_call() {
|
||||
/* flight controller client call if requested from Buzz*/
|
||||
/*FC call for takeoff,land, gohome and arm/disarm*/
|
||||
int tmp = buzzuav_closures::bzz_cmd();
|
||||
double* goto_pos = buzzuav_closures::getgoto();
|
||||
if (tmp == 1){
|
||||
double* goto_pos = buzzuav_closures::getgoto();
|
||||
if (tmp == 1) {
|
||||
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*/
|
||||
cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||
//std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
|
||||
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
|
||||
switch(buzzuav_closures::getcmd()){
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
if(current_mode != "LAND")
|
||||
SetMode("LAND", 0);
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
if(current_mode != "GUIDED")
|
||||
SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
|
||||
break;
|
||||
}
|
||||
|
||||
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.param5 = goto_pos[0];
|
||||
cmd_srv.request.param6 = goto_pos[1];
|
||||
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");
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
|
||||
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");
|
||||
cmd_srv.request.param6 = goto_pos[1];
|
||||
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");
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
|
||||
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 == 3) { /*FC call for arm*/
|
||||
armstate=1;
|
||||
Arm();
|
||||
} else if (tmp == 4){
|
||||
armstate=0;
|
||||
SetMode("LOITER", 0);
|
||||
armstate = 1;
|
||||
Arm();
|
||||
} else if (tmp == 5) { /*Buzz call for moveto*/
|
||||
} else if (tmp == 4) {
|
||||
armstate = 0;
|
||||
SetMode("LOITER", 0);
|
||||
Arm();
|
||||
} else if (tmp == 5) { /*Buzz call for moveto*/
|
||||
/*prepare the goto publish message */
|
||||
mavros_msgs::PositionTarget pt;
|
||||
pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX && mavros_msgs::PositionTarget::IGNORE_VY && mavros_msgs::PositionTarget::IGNORE_VZ && mavros_msgs::PositionTarget::IGNORE_AFX && mavros_msgs::PositionTarget::IGNORE_AFY && mavros_msgs::PositionTarget::IGNORE_AFZ && mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
|
||||
pt.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
|
||||
pt.position.x = goto_pos[0];
|
||||
pt.position.y = goto_pos[1];
|
||||
pt.position.z = goto_pos[2];
|
||||
pt.yaw = 0;//goto_pos[3];
|
||||
localsetpoint_pub.publish(pt);
|
||||
}
|
||||
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------
|
||||
/Refresh neighbours Position for every ten step
|
||||
|
|
@ -575,7 +571,6 @@ namespace rosbzz_node{
|
|||
bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
||||
mavros_msgs::CommandLong::Response &res){
|
||||
int rc_cmd;
|
||||
|
||||
switch(req.command){
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||
|
|
@ -584,7 +579,7 @@ namespace rosbzz_node{
|
|||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
ROS_INFO("RC_Call: LAND!!!!");
|
||||
ROS_INFO("RC_Call: LAND!!!! sending land");
|
||||
rc_cmd=mavros_msgs::CommandCode::NAV_LAND;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
|
|
@ -616,8 +611,8 @@ namespace rosbzz_node{
|
|||
rc_goto[0] = req.param5;
|
||||
rc_goto[1] = req.param6;
|
||||
rc_goto[2] = req.param7;
|
||||
// param 4 for yaw
|
||||
LocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], req.param4);
|
||||
// for test
|
||||
//SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0);
|
||||
|
||||
buzzuav_closures::rc_set_goto(rc_goto);
|
||||
rc_cmd= mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
|
|
@ -635,30 +630,18 @@ namespace rosbzz_node{
|
|||
/ TODO: check for integrity of this subscriber call back
|
||||
/----------------------------------------------------*/
|
||||
void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
|
||||
robot_id=(int)msg->data;
|
||||
|
||||
robot_id=(int)msg->data;
|
||||
}
|
||||
|
||||
/*
|
||||
* SOLO SPECIFIC FUNCTIONS
|
||||
*/
|
||||
|
||||
void roscontroller::LocalPosition(float x, float y, float z, float yaw){
|
||||
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
|
||||
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
|
||||
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
|
||||
|
||||
mavros_msgs::PositionTarget moveMsg;
|
||||
/*
|
||||
#define C_EARTH (double) 6378137.0
|
||||
|
||||
double d_lon = destination_longitude - global_position.longitude;
|
||||
double d_lat = destination_latitude - global_position.latitude;
|
||||
|
||||
flight_ctrl_data.x = ((d_lat) * M_PI/180.0) * C_EARTH;
|
||||
flight_ctrl_data.y = ((d_lon) * M_PI/180.0) * C_EARTH * cos((dst_latitude)*M_PI/180.0);
|
||||
*/
|
||||
|
||||
|
||||
moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
|
||||
moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
|
||||
mavros_msgs::PositionTarget::IGNORE_AFY |
|
||||
|
|
@ -666,22 +649,20 @@ namespace rosbzz_node{
|
|||
mavros_msgs::PositionTarget::IGNORE_VX |
|
||||
mavros_msgs::PositionTarget::IGNORE_VY |
|
||||
mavros_msgs::PositionTarget::IGNORE_VZ;
|
||||
|
||||
moveMsg.header.stamp = ros::Time::now();
|
||||
moveMsg.position.x = x;
|
||||
moveMsg.position.y = y;
|
||||
moveMsg.position.z = z;
|
||||
moveMsg.yaw = yaw * 0.0174532925; //DEG to RAD
|
||||
//moveMsg.yaw_rate = 1;
|
||||
moveMsg.yaw = 0;
|
||||
|
||||
localsetpoint_pub.publish(moveMsg);
|
||||
|
||||
local_position_pub.publish(moveMsg);
|
||||
ROS_INFO("Sent message!");
|
||||
ROS_INFO("Sent local position message!");
|
||||
}
|
||||
|
||||
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
|
||||
// wait if necessary
|
||||
if (delay_miliseconds > 0){
|
||||
if (delay_miliseconds != 0){
|
||||
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
|
||||
}
|
||||
// set mode
|
||||
|
|
|
|||
|
|
@ -132,8 +132,7 @@ function takeoff() {
|
|||
}
|
||||
function land() {
|
||||
log("Land: ", flight.status)
|
||||
# SOLO: land = status 4, guided = 1
|
||||
if(flight.status == 1){
|
||||
if(flight.status == 2 or flight.status == 1){
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
}
|
||||
|
|
@ -186,4 +185,4 @@ function reset() {
|
|||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue