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

for testing #2 - waypoints included

This commit is contained in:
isvogor 2017-02-21 19:20:38 -05:00
parent f01804b5a3
commit 24defade59
3 changed files with 549 additions and 546 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/roscontroller.h
View File

@ -160,7 +160,7 @@ private:
void Subscribe(ros::NodeHandle n_c); void Subscribe(ros::NodeHandle n_c);
void WaypointMissionSetup(); void WaypointMissionSetup(float lat, float lng, float alg);
}; };

16
src/buzz_utility.cpp
View File

@ -29,10 +29,7 @@ namespace buzz_utility{
/* Get robot id and update neighbor information */ /* Get robot id and update neighbor information */
map<int, Pos_struct>::iterator it; map<int, Pos_struct>::iterator it;
for (it = neighbours_pos_map.begin(); it != neighbours_pos_map.end(); ++it) { for (it = neighbours_pos_map.begin(); it != neighbours_pos_map.end(); ++it) {
buzzneighbors_add(VM, buzzneighbors_add(VM, it->first, (it->second).x, (it->second).y,
it->first,
(it->second).x,
(it->second).y,
(it->second).z); (it->second).z);
} }
} }
@ -158,13 +155,12 @@ namespace buzz_utility{
/* Send messages from FIFO */ /* Send messages from FIFO */
do { do {
/* Are there more messages? */ /* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break; if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */ /* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */ /* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) if (tot + buzzmsg_payload_size(m) + sizeof(uint16_t) > MSG_SIZE) {
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
break; break;
} }
@ -280,7 +276,6 @@ namespace buzz_utility{
return BUZZVM_STATE_READY; return BUZZVM_STATE_READY;
} }
/****************************************/ /****************************************/
/*Sets the .bzz and .bdbg file*/ /*Sets the .bzz and .bdbg file*/
/****************************************/ /****************************************/
@ -541,7 +536,6 @@ void buzz_script_destroy() {
fprintf(stdout, "Script execution stopped.\n"); fprintf(stdout, "Script execution stopped.\n");
} }
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -586,5 +580,3 @@ return VM;
} }

31
src/roscontroller.cpp
View File

@ -238,7 +238,7 @@ namespace rosbzz_node{
} }
} }
void roscontroller::WaypointMissionSetup(){ void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
mavros_msgs::WaypointPush srv; mavros_msgs::WaypointPush srv;
mavros_msgs::Waypoint waypoint; mavros_msgs::Waypoint waypoint;
@ -247,10 +247,9 @@ namespace rosbzz_node{
waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT; waypoint.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
waypoint.is_current = 2; // IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter waypoint.is_current = 2; // IMPORTANT: goto is no longer used, so instead, use magic number 2 for is_current parameter
waypoint.autocontinue = 0; waypoint.autocontinue = 0;
waypoint.x_lat = 45.507730f; waypoint.x_lat = lat;
waypoint.y_long = -73.613961f; waypoint.y_long = lng;
//45.507730, -73.613961 waypoint.z_alt = alt;
waypoint.z_alt = 10;
srv.request.waypoints.push_back(waypoint); srv.request.waypoints.push_back(waypoint);
if(mission_client.call(srv)){ if(mission_client.call(srv)){
@ -731,10 +730,10 @@ namespace rosbzz_node{
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */ /* arming */
SetMode("GUIDED", 0); SetMode("LOITER", 0);
//SetMode("LOITER", 0); //SetMode("GUIDED", 0);
cout << "this..." << endl; cout << "this..." << endl;
//SetModeAsync("GUIDED", 5000); SetModeAsync("GUIDED", 2000);
Arm(); Arm();
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
@ -754,12 +753,24 @@ namespace rosbzz_node{
case mavros_msgs::CommandCode::NAV_WAYPOINT: case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! "); ROS_INFO("RC_Call: GO TO!!!! ");
WaypointMissionSetup(); //WaypointMissionSetup();
double rc_goto[3]; double rc_goto[3];
rc_goto[0] = req.param5; rc_goto[0] = req.param5;
rc_goto[1] = req.param6; rc_goto[1] = req.param6;
rc_goto[2] = req.param7; rc_goto[2] = req.param7;
WaypointMissionSetup(req.param5, req.param6, req.param7);
/*
WaypointMissionSetup(45.505293f, -73.614722f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505324f, -73.614502f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505452f, -73.614655f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505463f, -73.614389f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
*/
buzzuav_closures::rc_set_goto(rc_goto); buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);