From 6fb86e7dd01c448a4c300e7c994635fddbe63ddb Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 5 Apr 2017 18:11:27 -0400 Subject: [PATCH] setpoint launch --- include/roscontroller.h | 2 +- launch/launch_config/m100.yaml | 1 + launch/launch_config/solo.yaml | 1 + src/roscontroller.cpp | 4 +++- 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index fa8b58f..8f85965 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -76,7 +76,7 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name; + std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; bool rcclient; diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml index 664b9b2..758aa8e 100644 --- a/launch/launch_config/m100.yaml +++ b/launch/launch_config/m100.yaml @@ -3,6 +3,7 @@ topics: battery : /power_status status : /flight_status fcclient : /dji_mavcmd + setpoint : /setpoint_raw/local armclient: /dji_mavarm modeclient: /dji_mavmode altitude: /rel_alt diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index d9f7961..a7e2e20 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -3,6 +3,7 @@ topics: battery : /mavros/battery status : /mavros/state fcclient: /mavros/cmd/command + setpoint: /mavros/setpoint_raw/local armclient: /mavros/cmd/arming modeclient: /mavros/set_mode stream: /mavros/set_stream_rate diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9ebc7f8..39819d3 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -170,6 +170,8 @@ namespace rosbzz_node{ /*Obtain fc client name from parameter server*/ if(node_handle.getParam("topics/fcclient", fcclient_name)); else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");} + if(node_handle.getParam("topics/setpoint", setpoint_name)); + else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/armclient", armclient)); else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/modeclient", modeclient)); @@ -195,7 +197,7 @@ namespace rosbzz_node{ /*publishers*/ payload_pub = n_c.advertise(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); - localsetpoint_pub = n_c.advertise("/mavros/setpoint_raw/local",1000); + localsetpoint_pub = n_c.advertise(setpoint_name,1000); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient);