rebase
This commit is contained in:
isvogor
commit
12ca11b275
|
|
@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
)
|
||||
|
||||
##############################
|
||||
#Generate messages#
|
||||
##############################
|
||||
|
||||
add_message_files(
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@
|
|||
#include "mavros_msgs/State.h"
|
||||
#include "mavros_msgs/BatteryStatus.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
#include "mavros_msgs/WaypointPush.h"
|
||||
#include "mavros_msgs/Waypoint.h"
|
||||
#include "sensor_msgs/NavSatStatus.h"
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <rosbuzz/neigh_pos.h>
|
||||
|
|
@ -82,6 +84,8 @@ private:
|
|||
mavros_msgs::SetMode m_cmdSetMode;
|
||||
ros::ServiceClient mode_client;
|
||||
|
||||
ros::ServiceClient mission_client;
|
||||
|
||||
void Initialize_pub_sub(ros::NodeHandle n_c);
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
|
|
@ -149,9 +153,16 @@ private:
|
|||
|
||||
void Arm();
|
||||
|
||||
void SetMode();
|
||||
void SetMode(std::string mode, int delay_miliseconds);
|
||||
|
||||
void SetModeAsync(std::string mode, int delay_miliseconds);
|
||||
|
||||
//void SetModeAsync(std::string mode, int delay);
|
||||
|
||||
void Subscribe(ros::NodeHandle n_c);
|
||||
|
||||
void WaypointMissionSetup(float lat, float lng, float alg);
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,47 @@
|
|||
<launch>
|
||||
<!-- RUN mavros -->
|
||||
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
|
||||
<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="$(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)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
</include>
|
||||
|
||||
<!-- run xbee -->
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="No_of_dev" type="int" value="3" />
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
|
||||
|
||||
<!-- set streaming rate
|
||||
/mavros/cmd/arming
|
||||
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
|
||||
-->
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
<launch>
|
||||
<!-- RUN mavros -->
|
||||
<arg name="fcu_url" default="/dev/ttyAMA0: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="$(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)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
</include>
|
||||
|
||||
<!-- set streaming rate -->
|
||||
<node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" />
|
||||
|
||||
<!-- run xbee -->
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="No_of_dev" type="int" value="3" />
|
||||
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -2,15 +2,14 @@
|
|||
|
||||
<launch>
|
||||
<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"/>
|
||||
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
|
||||
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,20 @@
|
|||
#! /bin/bash
|
||||
function takeoff {
|
||||
rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0
|
||||
}
|
||||
function land {
|
||||
rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0
|
||||
}
|
||||
function record {
|
||||
rosbag record /flight_status /global_position /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front_depth_image/compressedDepth /guidance/right_depth_image/compressedDepth /guidance/left_depth_image/compressedDepth /guidance/bottom_depth_image/compressedDepth /guidance/back_depth_image/compressedDepth
|
||||
}
|
||||
function clean {
|
||||
sudo rm /var/log/upstart/robot*
|
||||
sudo rm /var/log/upstart/x3s*
|
||||
}
|
||||
function startrobot {
|
||||
sudo service robot start
|
||||
}
|
||||
function stoprobot {
|
||||
sudo service robot stop
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,4 +1,5 @@
|
|||
#include "roscontroller.h"
|
||||
#include <thread>
|
||||
|
||||
namespace rosbzz_node{
|
||||
|
||||
|
|
@ -161,6 +162,7 @@ namespace rosbzz_node{
|
|||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
|
||||
/* Service Clients*/
|
||||
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);
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||
|
||||
|
|
@ -224,10 +226,37 @@ namespace rosbzz_node{
|
|||
}
|
||||
}
|
||||
|
||||
void roscontroller::SetMode(){
|
||||
void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
|
||||
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 = lat;
|
||||
waypoint.y_long = lng;
|
||||
waypoint.z_alt = alt;
|
||||
|
||||
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){
|
||||
// wait if necessary
|
||||
if (delay_miliseconds > 0){
|
||||
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
|
||||
}
|
||||
// set mode
|
||||
mavros_msgs::SetMode set_mode_message;
|
||||
set_mode_message.request.base_mode = 0;
|
||||
set_mode_message.request.custom_mode = "GUIDED"; // shit!
|
||||
set_mode_message.request.custom_mode = mode;
|
||||
if(mode_client.call(set_mode_message)) {
|
||||
ROS_INFO("Set Mode Service call successful!");
|
||||
} else {
|
||||
|
|
@ -235,6 +264,12 @@ namespace rosbzz_node{
|
|||
}
|
||||
}
|
||||
|
||||
//todo: this
|
||||
void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
|
||||
std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
|
||||
cout << "Async call called... " <<endl;
|
||||
}
|
||||
|
||||
|
||||
/*Prepare messages for each step and publish*/
|
||||
/*******************************************************************************************************/
|
||||
|
|
@ -566,7 +601,7 @@ namespace rosbzz_node{
|
|||
buzzuav_closures::flight_status_update(1);
|
||||
else if (msg->mode == "LAND")
|
||||
buzzuav_closures::flight_status_update(4);
|
||||
else // ground standby = LOITER?
|
||||
else
|
||||
buzzuav_closures::flight_status_update(1);//?
|
||||
}
|
||||
|
||||
|
|
@ -685,11 +720,19 @@ namespace rosbzz_node{
|
|||
ROS_INFO("RC_call: TAKE OFF!!!!");
|
||||
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
|
||||
/* arming */
|
||||
<<<<<<< HEAD
|
||||
SetMode();
|
||||
if(!armstate) {
|
||||
armstate =1;
|
||||
Arm();
|
||||
}
|
||||
=======
|
||||
SetMode("LOITER", 0);
|
||||
//SetMode("GUIDED", 0);
|
||||
cout << "this..." << endl;
|
||||
SetModeAsync("GUIDED", 2000);
|
||||
Arm();
|
||||
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
|
|
@ -717,11 +760,25 @@ namespace rosbzz_node{
|
|||
break;
|
||||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||
ROS_INFO("RC_Call: GO TO!!!! ");
|
||||
double rc_goto[3];
|
||||
|
||||
//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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue