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

for testing #2 - waypoints included

Browse Source
This commit is contained in:
isvogor 2017-02-21 19:20:38 -05:00
parent 321a1c84a4
commit 4f9a891216
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 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 */
map<int, Pos_struct>::iterator it;
for (it = neighbours_pos_map.begin(); it != neighbours_pos_map.end(); ++it) {
buzzneighbors_add(VM,
it->first,
(it->second).x,
(it->second).y,
buzzneighbors_add(VM, it->first, (it->second).x, (it->second).y,
(it->second).z);
}
}
@ -158,13 +155,12 @@ namespace buzz_utility{
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */
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 */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
if (tot + buzzmsg_payload_size(m) + sizeof(uint16_t) > MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
@ -280,7 +276,6 @@ namespace buzz_utility{
return BUZZVM_STATE_READY;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
@ -541,7 +536,6 @@ void buzz_script_destroy() {
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::Waypoint waypoint;
@ -247,10 +247,9 @@ namespace rosbzz_node{
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.autocontinue = 0;
waypoint.x_lat = 45.507730f;
waypoint.y_long = -73.613961f;
//45.507730, -73.613961
waypoint.z_alt = 10;
waypoint.x_lat = lat;
waypoint.y_long = lng;
waypoint.z_alt = alt;
srv.request.waypoints.push_back(waypoint);
if(mission_client.call(srv)){
@ -731,10 +730,10 @@ namespace rosbzz_node{
ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */
SetMode("GUIDED", 0);
//SetMode("LOITER", 0);
SetMode("LOITER", 0);
//SetMode("GUIDED", 0);
cout << "this..." << endl;
//SetModeAsync("GUIDED", 5000);
SetModeAsync("GUIDED", 2000);
Arm();
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
@ -754,12 +753,24 @@ namespace rosbzz_node{
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
WaypointMissionSetup();
//WaypointMissionSetup();
double rc_goto[3];
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
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);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);