first waypoint implementation - sitl works
This commit is contained in:
parent
4b0e18b762
commit
321a1c84a4
|
@ -11,6 +11,8 @@
|
||||||
#include "mavros_msgs/State.h"
|
#include "mavros_msgs/State.h"
|
||||||
#include "mavros_msgs/BatteryStatus.h"
|
#include "mavros_msgs/BatteryStatus.h"
|
||||||
#include "mavros_msgs/Mavlink.h"
|
#include "mavros_msgs/Mavlink.h"
|
||||||
|
#include "mavros_msgs/WaypointPush.h"
|
||||||
|
#include "mavros_msgs/Waypoint.h"
|
||||||
#include "sensor_msgs/NavSatStatus.h"
|
#include "sensor_msgs/NavSatStatus.h"
|
||||||
#include <sensor_msgs/LaserScan.h>
|
#include <sensor_msgs/LaserScan.h>
|
||||||
#include <rosbuzz/neigh_pos.h>
|
#include <rosbuzz/neigh_pos.h>
|
||||||
|
@ -81,6 +83,8 @@ private:
|
||||||
mavros_msgs::SetMode m_cmdSetMode;
|
mavros_msgs::SetMode m_cmdSetMode;
|
||||||
ros::ServiceClient mode_client;
|
ros::ServiceClient mode_client;
|
||||||
|
|
||||||
|
ros::ServiceClient mission_client;
|
||||||
|
|
||||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||||
|
|
||||||
/*Obtain data from ros parameter server*/
|
/*Obtain data from ros parameter server*/
|
||||||
|
@ -156,6 +160,8 @@ private:
|
||||||
|
|
||||||
void Subscribe(ros::NodeHandle n_c);
|
void Subscribe(ros::NodeHandle n_c);
|
||||||
|
|
||||||
|
void WaypointMissionSetup();
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -3,7 +3,7 @@
|
||||||
<launch>
|
<launch>
|
||||||
<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="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
|
<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
|
||||||
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
|
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/testalone.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"/>
|
||||||
|
|
|
@ -177,6 +177,7 @@ namespace rosbzz_node{
|
||||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||||
/* Service Clients*/
|
/* Service Clients*/
|
||||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||||
|
mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
|
||||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||||
|
|
||||||
|
@ -237,6 +238,29 @@ namespace rosbzz_node{
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void roscontroller::WaypointMissionSetup(){
|
||||||
|
mavros_msgs::WaypointPush srv;
|
||||||
|
mavros_msgs::Waypoint waypoint;
|
||||||
|
|
||||||
|
// prepare waypoint mission package
|
||||||
|
waypoint.frame = mavros_msgs::Waypoint::FRAME_GLOBAL;
|
||||||
|
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;
|
||||||
|
|
||||||
|
srv.request.waypoints.push_back(waypoint);
|
||||||
|
if(mission_client.call(srv)){
|
||||||
|
ROS_INFO("Mission service called!");
|
||||||
|
} else {
|
||||||
|
ROS_INFO("Mission service failed!");
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
|
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
|
||||||
// wait if necessary
|
// wait if necessary
|
||||||
if (delay_miliseconds > 0){
|
if (delay_miliseconds > 0){
|
||||||
|
@ -588,7 +612,7 @@ namespace rosbzz_node{
|
||||||
buzzuav_closures::flight_status_update(1);
|
buzzuav_closures::flight_status_update(1);
|
||||||
else if (msg->mode == "LAND")
|
else if (msg->mode == "LAND")
|
||||||
buzzuav_closures::flight_status_update(4);
|
buzzuav_closures::flight_status_update(4);
|
||||||
else // ground standby = LOITER?
|
else
|
||||||
buzzuav_closures::flight_status_update(1);//?
|
buzzuav_closures::flight_status_update(1);//?
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -708,8 +732,9 @@ namespace rosbzz_node{
|
||||||
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||||
/* arming */
|
/* arming */
|
||||||
SetMode("GUIDED", 0);
|
SetMode("GUIDED", 0);
|
||||||
cout<< "this..."<<endl;
|
//SetMode("LOITER", 0);
|
||||||
SetModeAsync("LAND", 5000);
|
cout << "this..." << endl;
|
||||||
|
//SetModeAsync("GUIDED", 5000);
|
||||||
Arm();
|
Arm();
|
||||||
buzzuav_closures::rc_call(rc_cmd);
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
res.success = true;
|
res.success = true;
|
||||||
|
@ -728,11 +753,13 @@ namespace rosbzz_node{
|
||||||
break;
|
break;
|
||||||
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();
|
||||||
|
|
||||||
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;
|
||||||
|
|
||||||
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);
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
# We need this for 2D vectors
|
# We need this for 2D vectors
|
||||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||||
include "/home/ivan/dev/buzz/src/include/vec2.bzz"
|
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
||||||
####################################################################################################
|
####################################################################################################
|
||||||
# Updater related
|
# Updater related
|
||||||
# This should be here for the updater to work, changing position of code will crash the updater
|
# This should be here for the updater to work, changing position of code will crash the updater
|
||||||
|
|
Loading…
Reference in New Issue