diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 7e7aaaf9..292d59ad 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -61,6 +61,7 @@ std::shared_ptr transform_broadcaster; std::shared_ptr static_transform_broadcaster; // Parameters +string mavros; string local_frame; string fcu_frame; ros::Duration transform_timeout; @@ -861,8 +862,9 @@ int main(int argc, char **argv) static_transform_broadcaster = std::make_shared(); // Params - nh.param("mavros/local_position/tf/frame_id", local_frame, "map"); - nh.param("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link"); + nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections + nh.param(mavros + "/local_position/tf/frame_id", local_frame, "map"); + nh.param(mavros + "/local_position/tf/child_frame_id", fcu_frame, "base_link"); nh_priv.param("target_frame", target.child_frame_id, string("navigate_target")); nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint")); nh_priv.param("auto_release", auto_release, true); @@ -894,25 +896,25 @@ int main(int argc, char **argv) arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0)); // Service clients - arming = nh.serviceClient("mavros/cmd/arming"); - set_mode = nh.serviceClient("mavros/set_mode"); + arming = nh.serviceClient(mavros + "/cmd/arming"); + set_mode = nh.serviceClient(mavros + "/set_mode"); // Telemetry subscribers - auto state_sub = nh.subscribe("mavros/state", 1, &handleState); - auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage); - auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage); - auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage); - auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage); - auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage); - auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition); + auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState); + auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage); + auto global_position_sub = nh.subscribe(mavros + "/global_position/global", 1, &handleMessage); + auto battery_sub = nh.subscribe(mavros + "/battery", 1, &handleMessage); + auto statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage); + auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage); + auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition); // Setpoint publishers - position_pub = nh.advertise("mavros/setpoint_position/local", 1); - position_raw_pub = nh.advertise("mavros/setpoint_raw/local", 1); - attitude_pub = nh.advertise("mavros/setpoint_attitude/attitude", 1); - attitude_raw_pub = nh.advertise("mavros/setpoint_raw/attitude", 1); - rates_pub = nh.advertise("mavros/setpoint_attitude/cmd_vel", 1); - thrust_pub = nh.advertise("mavros/setpoint_attitude/thrust", 1); + position_pub = nh.advertise(mavros + "/setpoint_position/local", 1); + position_raw_pub = nh.advertise(mavros + "/setpoint_raw/local", 1); + attitude_pub = nh.advertise(mavros + "/setpoint_attitude/attitude", 1); + attitude_raw_pub = nh.advertise(mavros + "/setpoint_raw/attitude", 1); + rates_pub = nh.advertise(mavros + "/setpoint_attitude/cmd_vel", 1); + thrust_pub = nh.advertise(mavros + "/setpoint_attitude/thrust", 1); // Service servers auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);