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

rebase

Browse Source
This commit is contained in:
isvogor 2017-02-23 19:39:59 -05:00
commit 12ca11b275
8 changed files with 714 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
) )
############################## ##############################
#Generate messages#
############################## ##############################
add_message_files( add_message_files(

13
include/roscontroller.h
View File

@ -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>
@ -82,6 +84,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*/
@ -149,9 +153,16 @@ private:
void Arm(); 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 Subscribe(ros::NodeHandle n_c);
void WaypointMissionSetup(float lat, float lng, float alg);
}; };
} }

47
launch/rosbuzz-testing-sitl.launch Normal file
View File

@ -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>

44
launch/rosbuzz-testing.launch Normal file
View File

@ -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>

9
launch/rosbuzz.launch
View File

@ -2,15 +2,14 @@
<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="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" /> <param name="bzzfile_name" value="/home/ivan/catkin_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"/>
<param name="out_payload" value="/outMavlink"/> <param name="out_payload" value="/outMavlink"/>
<param name="robot_id" value="3"/> <param name="robot_id" value="3"/>
<param name="No_of_Robots" 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> </node>
</launch> </launch>

20
misc/cmdlinectr.sh Normal file
View File

@ -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
}

1000
src/buzz_utility.cpp
View File

File diff suppressed because it is too large Load Diff

65
src/roscontroller.cpp
View File

@ -1,4 +1,5 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread>
namespace rosbzz_node{ namespace rosbzz_node{
@ -161,6 +162,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);
@ -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; mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0; 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)) { if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!"); ROS_INFO("Set Mode Service call successful!");
} else { } 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*/ /*Prepare messages for each step and publish*/
/*******************************************************************************************************/ /*******************************************************************************************************/
@ -566,7 +601,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);//?
} }
@ -685,11 +720,19 @@ 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 */
<<<<<<< HEAD
SetMode(); SetMode();
if(!armstate) { if(!armstate) {
armstate =1; armstate =1;
Arm(); Arm();
} }
=======
SetMode("LOITER", 0);
//SetMode("GUIDED", 0);
cout << "this..." << endl;
SetModeAsync("GUIDED", 2000);
Arm();
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
@ -717,11 +760,25 @@ 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!!!! ");
double rc_goto[3];
//WaypointMissionSetup();
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);