Header file corrections and inline method corrections commit
This commit is contained in:
parent
ba96337f5d
commit
63f2349618
|
@ -6,7 +6,7 @@
|
|||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
|
||||
#include "buzz_utility.h"
|
||||
#include <buzz_utility.h>
|
||||
|
||||
namespace buzz_utility{
|
||||
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#pragma once
|
||||
#include <stdio.h>
|
||||
#include "buzz_utility.h"
|
||||
#include "buzzuav_closures.h"
|
||||
#include <buzz_utility.h>
|
||||
#include <buzzuav_closures.h>
|
||||
#include <buzz/buzzdebug.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
|
|
@ -3,9 +3,9 @@
|
|||
//#define BUZZUAV_CLOSURES_H
|
||||
#include <buzz/buzzvm.h>
|
||||
#include <stdio.h>
|
||||
#include "uav_utility.h"
|
||||
#include "mavros_msgs/CommandCode.h"
|
||||
#include "ros/ros.h"
|
||||
#include <uav_utility.h>
|
||||
#include <mavros_msgs/CommandCode.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
namespace buzzuav_closures{
|
||||
|
||||
|
|
|
@ -98,7 +98,7 @@ namespace rosbzz_node{
|
|||
system("bzzasm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
|
||||
}
|
||||
|
||||
inline void roscontroller::prepare_msg_and_publish(){
|
||||
void roscontroller::prepare_msg_and_publish(){
|
||||
/*prepare the goto publish message */
|
||||
double* goto_pos = buzzuav_closures::getgoto();
|
||||
cmd_srv.request.param1 = goto_pos[0];
|
||||
|
@ -138,7 +138,7 @@ namespace rosbzz_node{
|
|||
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
inline void roscontroller::maintain_pos(int tim_step){
|
||||
void roscontroller::maintain_pos(int tim_step){
|
||||
if(timer_step >=10){
|
||||
neighbours_pos_map.clear();
|
||||
timer_step=0;
|
||||
|
@ -146,7 +146,7 @@ namespace rosbzz_node{
|
|||
}
|
||||
|
||||
/*Maintain neighbours position*/
|
||||
inline void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){
|
||||
void roscontroller::neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ){
|
||||
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
||||
if(it!=neighbours_pos_map.end())
|
||||
neighbours_pos_map.erase(it);
|
||||
|
@ -155,7 +155,7 @@ namespace rosbzz_node{
|
|||
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
inline void roscontroller::set_cur_pos(double latitude,
|
||||
void roscontroller::set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude){
|
||||
cur_pos [0] =latitude;
|
||||
|
@ -165,7 +165,7 @@ namespace rosbzz_node{
|
|||
}
|
||||
|
||||
/*convert from catresian to spherical coordinate system callback */
|
||||
inline double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||
double* roscontroller::cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||
double latitude,longitude,altitude;
|
||||
latitude=neighbours_pos_payload[0];
|
||||
longitude = neighbours_pos_payload[1];
|
||||
|
@ -177,17 +177,17 @@ namespace rosbzz_node{
|
|||
}
|
||||
|
||||
/*battery status callback*/
|
||||
inline void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
|
||||
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
|
||||
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
|
||||
}
|
||||
|
||||
/*current position callback*/
|
||||
inline void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
|
||||
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
|
||||
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
||||
}
|
||||
|
||||
/*payload callback callback*/
|
||||
inline void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
||||
uint64_t message_obt[msg->payload64.size()];
|
||||
int i = 0;
|
||||
|
||||
|
@ -216,7 +216,7 @@ namespace rosbzz_node{
|
|||
}
|
||||
|
||||
/* RC command service */
|
||||
inline bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||
bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request &req,
|
||||
mavros_msgs::CommandInt::Response &res){
|
||||
int rc_cmd;
|
||||
if(req.command==oldcmdID)
|
||||
|
|
|
@ -1,18 +1,18 @@
|
|||
#pragma once
|
||||
#include "ros/ros.h"
|
||||
#include "sensor_msgs/NavSatFix.h"
|
||||
#include "mavros_msgs/GlobalPositionTarget.h"
|
||||
#include "mavros_msgs/CommandCode.h"
|
||||
#include "mavros_msgs/CommandInt.h"
|
||||
#include "mavros_msgs/State.h"
|
||||
#include "mavros_msgs/BatteryStatus.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
#include "sensor_msgs/NavSatStatus.h"
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <mavros_msgs/GlobalPositionTarget.h>
|
||||
#include <mavros_msgs/CommandCode.h>
|
||||
#include <mavros_msgs/CommandInt.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/BatteryStatus.h>
|
||||
#include <mavros_msgs/Mavlink.h>
|
||||
#include <sensor_msgs/NavSatStatus.h>
|
||||
#include <sstream>
|
||||
#include <buzz/buzzasm.h>
|
||||
#include "buzzuav_closures.h"
|
||||
#include "buzz_utility.h"
|
||||
#include "uav_utility.h"
|
||||
#include <buzzuav_closures.h>
|
||||
#include <buzz_utility.h>
|
||||
#include <uav_utility.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
@ -49,13 +49,13 @@ private:
|
|||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
/*Create node Handler*/
|
||||
ros::NodeHandle n_c;
|
||||
ros::NodeHandle n_c;
|
||||
/*Commands for flight controller*/
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
/* The bytecode filename */
|
||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
/* The debugging information file name */
|
||||
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
|
||||
/* The debugging information file name */
|
||||
char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
|
||||
|
||||
void Initialize_pub_sub();
|
||||
|
||||
|
@ -65,27 +65,27 @@ private:
|
|||
void Compile_bzz();
|
||||
|
||||
/*Prepare messages and publish*/
|
||||
inline void prepare_msg_and_publish();
|
||||
void prepare_msg_and_publish();
|
||||
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
inline void maintain_pos(int tim_step);
|
||||
void maintain_pos(int tim_step);
|
||||
|
||||
/*Maintain neighbours position*/
|
||||
inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
|
||||
void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
inline void set_cur_pos(double latitude,
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from catresian to spherical coordinate system callback */
|
||||
inline double* cvt_spherical_coordinates(double neighbours_pos_payload []);
|
||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []);
|
||||
|
||||
/*battery status callback*/
|
||||
inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
||||
/*current position callback*/
|
||||
inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
|
||||
/*payload callback callback*/
|
||||
inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
|
Loading…
Reference in New Issue