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{ namespace buzzuav_closures{
typedef enum { typedef enum {
COMMAND_NIL = 0, // Dummy command COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off COMMAND_TAKEOFF, // Take off
COMMAND_LAND, COMMAND_LAND,
COMMAND_GOHOME, COMMAND_GOHOME,
COMMAND_ARM, COMMAND_ARM,
@ -64,7 +64,7 @@ void set_obstacle_dist(float dist[]);
*/ */
int buzzuav_takeoff(buzzvm_t vm); int buzzuav_takeoff(buzzvm_t vm);
/* /*
* Arm command from Buzz * Arm command from Buzz
*/ */
int buzzuav_arm(buzzvm_t vm); 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_update_targets(buzzvm_t vm);
int buzzuav_addtargetRB(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.status for flight status
* use flight.rc_cmd for current rc cmd * use flight.rc_cmd for current rc cmd
*/ */

310
include/roscontroller.h
View File

@ -42,198 +42,208 @@
using namespace std; using namespace std;
namespace rosbzz_node{ namespace rosbzz_node
{
class roscontroller{
class roscontroller
{
public: public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller(); ~roscontroller();
//void RosControllerInit(); //void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
private: private:
struct num_robot_count struct num_robot_count
{ {
uint8_t history[10]; uint8_t history[10];
uint8_t index=0; uint8_t index = 0;
uint8_t current=0; uint8_t current = 0;
num_robot_count(){} 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 struct gps
{ {
double longitude=0.0; double longitude=0.0;
double latitude=0.0; double latitude=0.0;
float altitude=0.0; float altitude=0.0;
}; typedef struct gps GPS ; }; typedef struct gps GPS ; // not useful in cpp
GPS target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload; GPS target, home, cur_pos;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; double cur_rel_altitude;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; uint64_t payload;
int timer_step=0; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int robot_id=0; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::string robot_name = ""; //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 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; int rc_cmd;
ros::Subscriber local_pos_sub; float fcu_timeout;
double local_pos_new[3]; 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; int setpoint_counter;
double my_x = 0, my_y = 0; double my_x = 0, my_y = 0;
std::ofstream log;
/*Commands for flight controller*/ std::ofstream log;
//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; /*Commands for flight controller*/
ros::ServiceClient arm_client; //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; mavros_msgs::CommandBool m_cmdBool;
ros::ServiceClient mode_client; ros::ServiceClient arm_client;
/*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 mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
/*Obtain data from ros parameter server*/ /*Initialize publisher and subscriber, done in the constructor*/
void Rosparameters_get(ros::NodeHandle& n_c_priv); void Initialize_pub_sub(ros::NodeHandle& n_c);
/*compiles buzz script from the specified .bzz file*/ std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
std::string Compile_bzz(std::string bzzfile_name);
/*Flight controller service call*/ /*Obtain data from ros parameter server*/
void flight_controller_service_call(); void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Prepare messages and publish*/ /*compiles buzz script from the specified .bzz file*/
void prepare_msg_and_publish(); std::string Compile_bzz(std::string bzzfile_name);
/*Flight controller service call*/
/*Refresh neighbours Position for every ten step*/ void flight_controller_service_call();
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/ /*Neighbours pos publisher*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); void neighbours_pos_publisher();
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ /*Prepare messages and publish*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); void prepare_msg_and_publish();
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude, /*Refresh neighbours Position for every ten step*/
double longitude, void maintain_pos(int tim_step);
double altitude);
/*convert from spherical to cartesian coordinate system callback */ /*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); float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]); void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t); void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y); void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(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_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat); double gps_r_lon, double gps_r_lat);
/*battery status callback*/ /*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
/*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*flight status callback*/ /*flight extended status callback*/
void flight_status_update(const mavros_msgs::State::ConstPtr& msg); void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
/*current position callback*/ /*flight status callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); 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); void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
/*payload callback callback*/ /*payload callback callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */ /* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
/*robot id sub callback*/ /*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/ /*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/ /*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle& node_handle); void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/ /*Arm/disarm method that can be called from buzz*/
void Arm(); void Arm();
/*set mode like guided for solo*/ /*set mode like guided for solo*/
void SetMode(std::string mode, int delay_miliseconds); void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle& n_c); 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> <launch>
<!-- RUN mavros --> <!-- 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="gcs_url" default="" />
<arg name="tgt_system" default="1" /> <arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" /> <arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" /> <arg name="log_output" default="screen" />
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch"> <include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" /> <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" /> <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" /> <arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_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_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" /> <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="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 --> <!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <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"/> <rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" /> <param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" /> <param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/> <param name="in_payload" value="inMavlink"/>
@ -50,7 +51,7 @@
<param name="xbee_status_srv" value="xbee_status"/> <param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/> <param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/> <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> </node>
</launch> </launch>

13
misc/cmdlinectr.sh
View File

@ -16,6 +16,7 @@ function testWP {
} }
function record { 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 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 { function clean {
sudo rm /var/log/upstart/robot* sudo rm /var/log/upstart/robot*
@ -23,10 +24,18 @@ function clean {
sudo rm /var/log/upstart/x3s* sudo rm /var/log/upstart/x3s*
} }
function startrobot { function startrobot {
sudo service robot start sudo service dji start
} }
function stoprobot { 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 { function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch # 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 0 -1 -1 -1
1 0 1000.0 0.0 1 0 1000.0 0.0
2 0 1000.0 1.57 2 0 1000.0 1.57
<<<<<<< HEAD
3 0 1000.0 3.14 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() uav_land()
} }
else { else {
<<<<<<< HEAD
barrier_set(ROBOTS,turnedoff,land) barrier_set(ROBOTS,turnedoff,land)
=======
barrier_set(ROBOTS,idle,land)
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
barrier_ready() barrier_ready()
timeW=0 timeW=0
#barrier = nil #barrier = nil
@ -58,6 +62,7 @@ function land() {
} }
} }
<<<<<<< HEAD
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
@ -74,6 +79,8 @@ function follow() {
} }
} }
=======
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
function uav_rccmd() { function uav_rccmd() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
@ -90,11 +97,17 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
<<<<<<< HEAD
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
log(rc_goto.latitude, " ", rc_goto.longitude) log(rc_goto.latitude, " ", rc_goto.longitude)
add_targetrb(0,rc_goto.latitude,rc_goto.longitude) add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow statef = follow
#uav_goto() #uav_goto()
=======
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_arm() uav_arm()

43
src/buzz_utility.cpp
View File

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

135
src/buzzuav_closures.cpp
View File

@ -1,7 +1,7 @@
/** @file buzzuav_closures.cpp /** @file buzzuav_closures.cpp
* @version 1.0 * @version 1.0
* @date 27.09.2016 * @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 * @author Vivek Shankar Varadharajan
* @copyright 2016 MistLab. All rights reserved. * @copyright 2016 MistLab. All rights reserved.
*/ */
@ -10,7 +10,6 @@
#include "math.h" #include "math.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr; //static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/ /*forward declaration of ros controller ptr storing function*/
@ -448,7 +447,7 @@ namespace buzzuav_closures{
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm); buzzvm_gstore(vm);
/* Fill into the proximity table */ /* Fill into the proximity table */
buzzobj_t tProxRead; buzzobj_t tProxRead;
float angle =0; float angle =0;
@ -458,80 +457,80 @@ namespace buzzuav_closures{
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ /* Fill in the read */
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i+1]); buzzvm_pushf(vm, obst[i+1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Store read table in the proximity table */ /* Store read table in the proximity table */
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_tput(vm); buzzvm_tput(vm);
angle+=1.5708; angle+=1.5708;
} }
/* Create table for bottom read */ /* Create table for bottom read */
angle =-1; angle =-1;
buzzvm_pusht(vm); buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ /* Fill in the read */
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]); buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/*Store read table in the proximity table*/ /*Store read table in the proximity table*/
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4); buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_tput(vm); buzzvm_tput(vm);
/* /*
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
buzzvm_pushf(vm, obst[0]); buzzvm_pushf(vm, obst[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
buzzvm_pushf(vm, obst[1]); buzzvm_pushf(vm, obst[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
buzzvm_pushf(vm, obst[2]); buzzvm_pushf(vm, obst[2]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
buzzvm_pushf(vm, obst[3]); buzzvm_pushf(vm, obst[3]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
buzzvm_pushf(vm, obst[4]); buzzvm_pushf(vm, obst[4]);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm);*/ buzzvm_gstore(vm);*/
return vm->state; 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){ //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
//roscontroller_ptr = roscontroller_ptrin; //roscontroller_ptr = roscontroller_ptrin;
//} //}
} }

2201
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff