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

merge rosbuzz

This commit is contained in:
dave 2017-06-26 14:35:15 -04:00
parent d7ca910e21 8e5acd264d
commit eb130be1bb
9 changed files with 1492 additions and 1254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/buzzuav_closures.h
View File

@ -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,
@ -64,7 +64,7 @@ void set_obstacle_dist(float dist[]);
*/
int buzzuav_takeoff(buzzvm_t vm);
/*
* Arm command from Buzz
* Arm command from Buzz
*/
int buzzuav_arm(buzzvm_t vm);
/*
@ -90,7 +90,7 @@ int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_update_targets(buzzvm_t vm);
int buzzuav_addtargetRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status
* use flight.rc_cmd for current rc cmd
*/

310
include/roscontroller.h
View File

@ -42,198 +42,208 @@
using namespace std;
namespace rosbzz_node{
class roscontroller{
namespace rosbzz_node
{
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(){}
}; typedef struct num_robot_count 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
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;
struct gps
{
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp
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 = "";
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 = "";
//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;
std::ofstream log;
int setpoint_counter;
double my_x = 0, my_y = 0;
/*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;
std::ofstream log;
mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client;
/*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::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient arm_client;
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
/*compiles buzz script from the specified .bzz file*/
std::string Compile_bzz(std::string bzzfile_name);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
/*Flight controller service call*/
void flight_controller_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*compiles buzz script from the specified .bzz file*/
std::string Compile_bzz(std::string bzzfile_name);
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Flight controller service call*/
void flight_controller_service_call();
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*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 );
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*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 */
/*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 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 */
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);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::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);
/*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);
/*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 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 SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots();
void GetRobotId();
};
}

17
launch/rosbuzz-solo.launch
View File

@ -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>

13
misc/cmdlinectr.sh
View File

@ -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

6
script/Graph_drone.graph
View File

@ -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
4 0 1000.0 4.71
=======
3 0 1000.0 3.14
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7

13
script/include/uavstates.bzz
View File

@ -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()

43
src/buzz_utility.cpp
View File

@ -1,7 +1,7 @@
/** @file buzz_utility.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved.
*/
@ -22,7 +22,7 @@ namespace buzz_utility{
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map;
/****************************************/
void add_user(int id, double latitude, double longitude, float altitude)
@ -172,12 +172,12 @@ namespace buzz_utility{
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
IN_MSG.push_back(pl);
}
void in_message_process(){
while(!IN_MSG.empty()){
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
/* Go through messages and append them to the FIFO */
uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
/*Size is at first 2 bytes*/
@ -211,7 +211,7 @@ namespace buzz_utility{
/***************************************************/
uint64_t* obt_out_msg(){
/* Process out messages */
buzzvm_process_outmsgs(VM);
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
@ -352,7 +352,7 @@ namespace buzz_utility{
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
@ -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);
@ -426,7 +426,7 @@ int create_stig_tables() {
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
@ -489,7 +489,8 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -498,10 +499,10 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -518,7 +519,7 @@ int create_stig_tables() {
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
@ -551,7 +552,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -560,7 +561,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -607,7 +608,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0;
}
/* Create vstig tables
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -700,12 +701,12 @@ int create_stig_tables() {
buzz_error_info());
buzzvm_dump(VM);
}
/*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
//int status = 1;
@ -763,7 +764,7 @@ int create_stig_tables() {
buzz_error_info());
fprintf(stdout, "step test VM state %i\n",a);
}
return a == BUZZVM_STATE_READY;
}
@ -777,5 +778,3 @@ int create_stig_tables() {
buzzvm_gstore(VM);
}
}

135
src/buzzuav_closures.cpp
View File

@ -1,7 +1,7 @@
/** @file buzzuav_closures.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
* @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved.
*/
@ -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*/
@ -448,7 +447,7 @@ namespace buzzuav_closures{
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm);
/* Fill into the proximity table */
buzzobj_t tProxRead;
float angle =0;
@ -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 */
/***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin;
//}
/***********************************************/
/* Store Ros controller object pointer */
/***********************************************/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin;
//}
}

2201
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff