merge rosbuzz
This commit is contained in:
dave
commit
eb130be1bb
|
|
@ -14,8 +14,8 @@
|
|||
|
||||
namespace buzzuav_closures{
|
||||
typedef enum {
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_LAND,
|
||||
COMMAND_GOHOME,
|
||||
COMMAND_ARM,
|
||||
|
|
|
|||
|
|
@ -42,198 +42,208 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace rosbzz_node{
|
||||
namespace rosbzz_node
|
||||
{
|
||||
|
||||
class roscontroller{
|
||||
class roscontroller
|
||||
{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
||||
private:
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index=0;
|
||||
uint8_t current=0;
|
||||
num_robot_count(){}
|
||||
struct num_robot_count
|
||||
{
|
||||
uint8_t history[10];
|
||||
uint8_t index = 0;
|
||||
uint8_t current = 0;
|
||||
num_robot_count(){}
|
||||
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
|
||||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ; // not useful in cpp
|
||||
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ;
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::string robot_name = "";
|
||||
//int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
uint8_t no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Subscriber relative_altitude_sub;
|
||||
|
||||
std::string local_pos_sub_name;
|
||||
ros::Subscriber local_pos_sub;
|
||||
double local_pos_new[3];
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
uint8_t no_of_robots=0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0 ;
|
||||
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
|
||||
std::string stream_client_name;
|
||||
std::string relative_altitude_sub_name;
|
||||
std::string setpoint_nonraw;
|
||||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
ros::Subscriber relative_altitude_sub;
|
||||
|
||||
std::string local_pos_sub_name;
|
||||
ros::Subscriber local_pos_sub;
|
||||
double local_pos_new[3];
|
||||
|
||||
|
||||
ros::ServiceClient stream_client;
|
||||
ros::ServiceClient stream_client;
|
||||
|
||||
int setpoint_counter;
|
||||
double my_x = 0, my_y = 0;
|
||||
int setpoint_counter;
|
||||
double my_x = 0, my_y = 0;
|
||||
|
||||
std::ofstream log;
|
||||
std::ofstream log;
|
||||
|
||||
/*Commands for flight controller*/
|
||||
//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;
|
||||
/*Commands for flight controller*/
|
||||
//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;
|
||||
|
||||
mavros_msgs::CommandBool m_cmdBool;
|
||||
ros::ServiceClient arm_client;
|
||||
mavros_msgs::CommandBool m_cmdBool;
|
||||
ros::ServiceClient arm_client;
|
||||
|
||||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
|
||||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
std::string Compile_bzz(std::string bzzfile_name);
|
||||
/*compiles buzz script from the specified .bzz file*/
|
||||
std::string Compile_bzz(std::string bzzfile_name);
|
||||
|
||||
/*Flight controller service call*/
|
||||
void flight_controller_service_call();
|
||||
/*Flight controller service call*/
|
||||
void flight_controller_service_call();
|
||||
|
||||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step);
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step);
|
||||
|
||||
/*Puts neighbours position inside neigbours_pos_map*/
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
/*Puts neighbours position inside neigbours_pos_map*/
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
float constrainAngle(float x);
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
||||
void gps_ned_home(float &ned_x, float &ned_y);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
||||
void gps_ned_home(float &ned_x, float &ned_y);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
|
||||
/*battery status callback*/
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
/*battery status callback */
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
|
||||
/*flight status callback*/
|
||||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
/*flight status callback*/
|
||||
void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
|
||||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
|
||||
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
|
||||
/*robot id sub callback*/
|
||||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
/*robot id sub callback*/
|
||||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
|
||||
/*Obstacle distance table callback*/
|
||||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
/*Obstacle distance table callback*/
|
||||
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
|
||||
|
||||
/*Get publisher and subscriber from YML file*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
/*Get publisher and subscriber from YML file*/
|
||||
void GetSubscriptionParameters(ros::NodeHandle& node_handle);
|
||||
|
||||
/*Arm/disarm method that can be called from buzz*/
|
||||
void Arm();
|
||||
/*Arm/disarm method that can be called from buzz*/
|
||||
void Arm();
|
||||
|
||||
/*set mode like guided for solo*/
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
/*set mode like guided for solo*/
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle& n_c);
|
||||
/*Robot independent subscribers*/
|
||||
void Subscribe(ros::NodeHandle& n_c);
|
||||
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
void fc_command_setup();
|
||||
void fc_command_setup();
|
||||
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
void SetLocalPosition(float x, float y, float z, float yaw);
|
||||
|
||||
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
|
||||
void SetLocalPositionNonRaw(float x, float y, float z, float yaw);
|
||||
|
||||
void SetStreamRate(int id, int rate, int on_off);
|
||||
void SetStreamRate(int id, int rate, int on_off);
|
||||
|
||||
void get_number_of_robots();
|
||||
|
||||
void GetRobotId();
|
||||
bool GetDequeFull(bool &result);
|
||||
bool GetRssi(float &result);
|
||||
bool TriggerAPIRssi(const uint8_t short_id);
|
||||
bool GetAPIRssi(const uint8_t short_id, float &result);
|
||||
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
||||
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
||||
|
||||
void get_number_of_robots();
|
||||
void GetRobotId();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
<launch>
|
||||
<!-- RUN mavros -->
|
||||
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
|
||||
<arg name="fcu_url" default="/dev/ttyS0:115200" />
|
||||
<arg name="gcs_url" default="" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="log_output" default="screen" />
|
||||
|
||||
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" />
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
|
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
|
|
@ -36,13 +36,14 @@
|
|||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
|
||||
|
||||
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
|
||||
<param name="Baud_rate" type="double" value="230400" />
|
||||
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" />
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
|
|
@ -50,7 +51,7 @@
|
|||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="xbee_plugged" value="true"/>
|
||||
<param name="name" value="solos1"/>
|
||||
<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/stand_by.bzz"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ function testWP {
|
|||
}
|
||||
function record {
|
||||
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
|
||||
|
||||
}
|
||||
function clean {
|
||||
sudo rm /var/log/upstart/robot*
|
||||
|
|
@ -23,10 +24,18 @@ function clean {
|
|||
sudo rm /var/log/upstart/x3s*
|
||||
}
|
||||
function startrobot {
|
||||
sudo service robot start
|
||||
sudo service dji start
|
||||
}
|
||||
function stoprobot {
|
||||
sudo service robot stop
|
||||
sudo service dji stop
|
||||
}
|
||||
function updaterobot {
|
||||
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
|
||||
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
|
||||
}
|
||||
function updaterobot {
|
||||
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
|
||||
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
|
||||
}
|
||||
function updaterobot {
|
||||
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
|
||||
|
|
|
|||
|
|
@ -1,5 +1,9 @@
|
|||
0 -1 -1 -1
|
||||
1 0 1000.0 0.0
|
||||
2 0 1000.0 1.57
|
||||
<<<<<<< HEAD
|
||||
3 0 1000.0 3.14
|
||||
4 0 1000.0 4.71
|
||||
=======
|
||||
3 0 1000.0 3.14
|
||||
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
|
||||
|
|
|
|||
|
|
@ -50,7 +50,11 @@ function land() {
|
|||
uav_land()
|
||||
}
|
||||
else {
|
||||
<<<<<<< HEAD
|
||||
barrier_set(ROBOTS,turnedoff,land)
|
||||
=======
|
||||
barrier_set(ROBOTS,idle,land)
|
||||
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
|
||||
barrier_ready()
|
||||
timeW=0
|
||||
#barrier = nil
|
||||
|
|
@ -58,6 +62,7 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
<<<<<<< HEAD
|
||||
function follow() {
|
||||
if(size(targets)>0) {
|
||||
UAVSTATE = "FOLLOW"
|
||||
|
|
@ -74,6 +79,8 @@ function follow() {
|
|||
}
|
||||
}
|
||||
|
||||
=======
|
||||
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
|
||||
function uav_rccmd() {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
|
|
@ -90,11 +97,17 @@ function uav_rccmd() {
|
|||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
<<<<<<< HEAD
|
||||
UAVSTATE = "FOLLOW"
|
||||
log(rc_goto.latitude, " ", rc_goto.longitude)
|
||||
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
|
||||
statef = follow
|
||||
#uav_goto()
|
||||
=======
|
||||
statef = idle
|
||||
#uav_goto()
|
||||
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
|
||||
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
|
|
|
|||
|
|
@ -405,7 +405,7 @@ int create_stig_tables() {
|
|||
//buzzvm_gstore(VM);
|
||||
//buzzvm_dump(VM);
|
||||
|
||||
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||
buzzvm_tget(VM);
|
||||
|
|
@ -489,6 +489,7 @@ int create_stig_tables() {
|
|||
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Create vstig tables
|
||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
|
|
@ -777,5 +778,3 @@ int create_stig_tables() {
|
|||
buzzvm_gstore(VM);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,6 @@
|
|||
#include "math.h"
|
||||
|
||||
namespace buzzuav_closures{
|
||||
|
||||
// TODO: Minimize the required global variables and put them in the header
|
||||
//static const rosbzz_node::roscontroller* roscontroller_ptr;
|
||||
/*forward declaration of ros controller ptr storing function*/
|
||||
|
|
@ -458,80 +457,80 @@ namespace buzzuav_closures{
|
|||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i+1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[i+1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/* Store read table in the proximity table */
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle+=1.5708;
|
||||
buzzvm_pushi(vm, i);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
angle+=1.5708;
|
||||
}
|
||||
/* Create table for bottom read */
|
||||
angle =-1;
|
||||
/* Create table for bottom read */
|
||||
angle =-1;
|
||||
buzzvm_pusht(vm);
|
||||
tProxRead = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
/* Fill in the read */
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
|
||||
buzzvm_pushf(vm, angle);
|
||||
buzzvm_tput(vm);
|
||||
/*Store read table in the proximity table*/
|
||||
buzzvm_push(vm, tProxTable);
|
||||
buzzvm_pushi(vm, 4);
|
||||
buzzvm_push(vm, tProxRead);
|
||||
buzzvm_tput(vm);
|
||||
|
||||
/*
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
|
||||
buzzvm_pushf(vm, obst[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
|
||||
buzzvm_pushf(vm, obst[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
|
||||
buzzvm_pushf(vm, obst[3]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
|
||||
buzzvm_pushf(vm, obst[4]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);*/
|
||||
return vm->state;
|
||||
}
|
||||
/*
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
|
||||
buzzvm_pushf(vm, obst[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
|
||||
buzzvm_pushf(vm, obst[1]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
|
||||
buzzvm_pushf(vm, obst[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
|
||||
buzzvm_pushf(vm, obst[3]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
|
||||
buzzvm_pushf(vm, obst[4]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_gstore(vm);*/
|
||||
return vm->state;
|
||||
}
|
||||
|
||||
/**********************************************/
|
||||
/*Dummy closure for use during update testing */
|
||||
/**********************************************/
|
||||
/**********************************************/
|
||||
/*Dummy closure for use during update testing */
|
||||
/**********************************************/
|
||||
|
||||
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
|
||||
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
|
||||
|
||||
/***********************************************/
|
||||
/* Store Ros controller object pointer */
|
||||
/***********************************************/
|
||||
/***********************************************/
|
||||
/* Store Ros controller object pointer */
|
||||
/***********************************************/
|
||||
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
|
||||
//roscontroller_ptr = roscontroller_ptrin;
|
||||
//}
|
||||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
|
||||
//roscontroller_ptr = roscontroller_ptrin;
|
||||
//}
|
||||
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue