Skip to content

Commit

Permalink
Use tf_prefix parameter for multiple drone settings (fixes AutonomyLa…
Browse files Browse the repository at this point in the history 
…b#156)

- Use tf_prefix to prefix tf frame names
- Update single and multiple drone launch files to demo the usage
  • Loading branch information
autolab committed Aug 11, 2015
1 parent 902f91c commit c8665fd
Show file tree
Hide file tree
Showing 5 changed files with 124 additions and 117 deletions.
2 changes: 1 addition & 1 deletion include/ardrone_autonomy/ardrone_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ class ARDroneDriver
* TF Frames
* Base: Should be COM
*/
std::string drone_frame_base, drone_frame_imu, drone_frame_front_cam, drone_frame_bottom_cam;
std::string tf_prefix, drone_frame_base, drone_frame_imu, drone_frame_front_cam, drone_frame_bottom_cam;
tf::StampedTransform tf_base_front, tf_base_bottom, tf_odom;

// Huge part of IMU message is constant, let's fill'em once.
Expand Down
54 changes: 26 additions & 28 deletions launch/ardrone.launch
View file
Original file line number Diff line number Diff line change
@@ -1,34 +1,32 @@
<?xml version="1.0"?>
<!-- This is a sample lanuch file, please change it based on your needs -->
<launch>
<!-- IPv4 address of your drone -->
<arg name="ip" default="192.168.1.1" />
<!-- IPv4 address of your drone -->
<arg name="ip" default="192.168.1.1" />
<!-- Ultrasound frequency (7 or 8). -->
<arg name="freq" default="8" />
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver"
output="screen" clear_params="true" args="-ip $(arg ip)">
<param name="outdoor" value="0" />
<param name="max_bitrate" value="4000" />
<param name="bitrate" value="4000" />
<param name="navdata_demo" value="0" />
<param name="flight_without_shell" value="0" />
<param name="altitude_max" value="4000" />
<param name="altitude_min" value="50" />
<param name="euler_angle_max" value="0.21" />
<param name="control_vz_max" value="700" />
<param name="control_yaw" value="1.75" />
<param name="detect_type" value="10" />
<param name="enemy_colors" value="3" />
<param name="detections_select_h" value="32" />
<param name="detections_select_v_hsync" value="128" />
<param name="enemy_without_shell" value="0" />
<param name="do_imu_caliberation" value="false" />
<param name="tf_prefix" value="mydrone" />
<param name="ultrasound_freq" value="$(arg freq)" />
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
</node>
<arg name="freq" default="8" />
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver"
output="screen" clear_params="true" args="-ip $(arg ip)">
<param name="outdoor" value="0" />
<param name="max_bitrate" value="4000" />
<param name="bitrate" value="4000" />
<param name="navdata_demo" value="0" />
<param name="flight_without_shell" value="0" />
<param name="altitude_max" value="4000" />
<param name="altitude_min" value="50" />
<param name="euler_angle_max" value="0.21" />
<param name="control_vz_max" value="700" />
<param name="control_yaw" value="1.75" />
<param name="detect_type" value="10" />
<param name="enemy_colors" value="3" />
<param name="detections_select_h" value="32" />
<param name="detections_select_v_hsync" value="128" />
<param name="enemy_without_shell" value="0" />
<param name="ultrasound_freq" value="$(arg freq)" />
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
</node>
</launch>

138 changes: 68 additions & 70 deletions launch/ardrone_aggressive.launch
View file
Original file line number Diff line number Diff line change
@@ -1,81 +1,79 @@
<?xml version="1.0"?>
<!-- This is a sample lanuch file, please change it based on your needs -->
<launch>
<arg name="ip" default="192.168.1.1" />
<arg name="freq" default="8" />
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver"
output="screen" clear_params="true" args="-ip $(arg ip)">

<!-- Modifies the drone's onboard parameters.
If not specified, drone default will be used (consult SDK or ardrone_autonomy wiki) -->
<param name="outdoor" value="1" />
<param name="flight_without_shell" value="1" />
<param name="navdata_demo" value="0" />
<arg name="ip" default="192.168.1.1" />
<arg name="freq" default="8" />
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver"
output="screen" clear_params="true" args="-ip $(arg ip)">

<param name="max_bitrate" value="2000" />
<param name="bitrate" value="2000" />
<param name="video_codec" value="129" />

<param name="altitude_max" value="5000" />
<param name="altitude_min" value="300" />
<!-- Modifies the drone's onboard parameters.
If not specified, drone default will be used (consult SDK or ardrone_autonomy wiki) -->
<param name="outdoor" value="1" />
<param name="flight_without_shell" value="1" />
<param name="navdata_demo" value="0" />

<param name="control_vz_max" value="2000" />
<param name="control_yaw" value="6.11" />
<param name="euler_angle_max" value="0.35" />
<param name="max_bitrate" value="2000" />
<param name="bitrate" value="2000" />
<param name="video_codec" value="129" />

<param name="detect_type" value="10" />
<param name="detections_select_h" value="128" />
<param name="detections_select_v_hsync" value="32" />
<param name="enemy_colors" value="3" />
<param name="enemy_without_shell" value="0" />
<param name="do_imu_caliberation" value="false" />
<param name="tf_prefix" value="mydrone" />
<param name="ultrasound_freq" value="$(arg freq)" />
<param name="altitude_max" value="5000" />
<param name="altitude_min" value="300" />

<!-- Enables the standard /ardrone/navdata, imu and mag topics. If not specified, defaults to TRUE -->
<param name="enable_legacy_navdata" value="true" />

<!-- Enables the new-style, full information navdata packets. If not specified, defaults to FALSE -->
<param name="enable_navdata_demo" value="true" />
<param name="enable_navdata_time" value="true" />
<param name="enable_navdata_raw_measures" value="true" />
<param name="enable_navdata_phys_measures" value="true" />
<param name="enable_navdata_gyros_offsets" value="true" />
<param name="enable_navdata_euler_angles" value="true" />
<param name="enable_navdata_references" value="true" />
<param name="enable_navdata_trims" value="true" />
<param name="enable_navdata_rc_references" value="true" />
<param name="enable_navdata_pwm" value="true" />
<param name="enable_navdata_altitude" value="true" />
<param name="enable_navdata_vision_raw" value="true" />
<param name="enable_navdata_vision_of" value="true" />
<param name="enable_navdata_vision" value="true" />
<param name="enable_navdata_vision_perf" value="true" />
<param name="enable_navdata_trackers_send" value="true" />
<param name="enable_navdata_vision_detect" value="true" />
<param name="enable_navdata_watchdog" value="true" />
<param name="enable_navdata_adc_data_frame" value="true" />
<param name="enable_navdata_video_stream" value="true" />
<param name="enable_navdata_games" value="true" />
<param name="enable_navdata_pressure_raw" value="true" />
<param name="enable_navdata_magneto" value="true" />
<param name="enable_navdata_wind_speed" value="true" />
<param name="enable_navdata_kalman_pressure" value="true" />
<param name="enable_navdata_hdvideo_stream" value="true" />
<param name="enable_navdata_wifi" value="true" />
<param name="enable_navdata_zimmu_3000" value="true" />
<param name="control_vz_max" value="2000" />
<param name="control_yaw" value="6.11" />
<param name="euler_angle_max" value="0.35" />

<!-- Tunes the speed at which the ros loop runs, and thus, the rate at which navdata is published -->
<param name="looprate" value="50" />
<param name="detect_type" value="10" />
<param name="detections_select_h" value="128" />
<param name="detections_select_v_hsync" value="32" />
<param name="enemy_colors" value="3" />
<param name="enemy_without_shell" value="0" />
<param name="ultrasound_freq" value="$(arg freq)" />

<!-- Do we want to publish new-style navdata when received (true), or every time the ros-loop runs (false)? -->
<!-- (does not affect legacy navdata, which is always published at ros-loop rate) -->
<param name="realtime_navdata" value="true" />
<param name="realtime_video" value="true" />
<!-- Enables the standard /ardrone/navdata, imu and mag topics. If not specified, defaults to TRUE -->
<param name="enable_legacy_navdata" value="true" />

<!-- Enables the new-style, full information navdata packets. If not specified, defaults to FALSE -->
<param name="enable_navdata_demo" value="true" />
<param name="enable_navdata_time" value="true" />
<param name="enable_navdata_raw_measures" value="true" />
<param name="enable_navdata_phys_measures" value="true" />
<param name="enable_navdata_gyros_offsets" value="true" />
<param name="enable_navdata_euler_angles" value="true" />
<param name="enable_navdata_references" value="true" />
<param name="enable_navdata_trims" value="true" />
<param name="enable_navdata_rc_references" value="true" />
<param name="enable_navdata_pwm" value="true" />
<param name="enable_navdata_altitude" value="true" />
<param name="enable_navdata_vision_raw" value="true" />
<param name="enable_navdata_vision_of" value="true" />
<param name="enable_navdata_vision" value="true" />
<param name="enable_navdata_vision_perf" value="true" />
<param name="enable_navdata_trackers_send" value="true" />
<param name="enable_navdata_vision_detect" value="true" />
<param name="enable_navdata_watchdog" value="true" />
<param name="enable_navdata_adc_data_frame" value="true" />
<param name="enable_navdata_video_stream" value="true" />
<param name="enable_navdata_games" value="true" />
<param name="enable_navdata_pressure_raw" value="true" />
<param name="enable_navdata_magneto" value="true" />
<param name="enable_navdata_wind_speed" value="true" />
<param name="enable_navdata_kalman_pressure" value="true" />
<param name="enable_navdata_hdvideo_stream" value="true" />
<param name="enable_navdata_wifi" value="true" />
<param name="enable_navdata_zimmu_3000" value="true" />

<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
</node>
<!-- Tunes the speed at which the ros loop runs, and thus, the rate at which navdata is published -->
<param name="looprate" value="50" />

<!-- Do we want to publish new-style navdata when received (true), or every time the ros-loop runs (false)? -->
<!-- (does not affect legacy navdata, which is always published at ros-loop rate) -->
<param name="realtime_navdata" value="true" />
<param name="realtime_video" value="true" />

<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
</node>
</launch>
33 changes: 20 additions & 13 deletions launch/ardrone_group.launch
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,26 @@
<?xml version="1.0"?>
<!-- Example launch file for multiple AR Drones -->
<launch>
<!-- Include the ardrone launch file for each drone.
IMPORTANT: Ensure the namespace for each include is unique. -->
<include file="$(find ardrone_autonomy)/launch/ardrone.launch" ns="ardrone1">
<arg name="ip" value="192.168.1.152" />
<!-- Choose ultrasound frequencies to help reduce interference between drones.
Available values are '7' or '8'. -->
<arg name="freq" value="7" />
</include>
<!-- Include the ardrone launch file for each drone.
IMPORTANT: Ensure the namespace for each include is unique. -->

<!-- Add second drone -->
<include file="$(find ardrone_autonomy)/launch/ardrone.launch" ns="ardrone2">
<arg name="ip" value="192.168.1.157" />
<arg name="freq" value="8" />
</include>
<group ns="ardrone1">
<param name="tf_prefix" value="ardrone1" />
<include file="$(find ardrone_autonomy)/launch/ardrone.launch">
<arg name="ip" value="192.168.1.152" />
<!-- Choose ultrasound frequencies to help reduce interference between drones.
Available values are '7' or '8'. -->
<arg name="freq" value="7" />
</include>
</group>

<!-- Add second drone -->
<group ns="ardrone2">
<param name="tf_prefix" value="ardrone2" />
<include file="$(find ardrone_autonomy)/launch/ardrone.launch">
<arg name="ip" value="192.168.1.157" />
<arg name="freq" value="8" />
</include>
</group>

</launch>
14 changes: 9 additions & 5 deletions src/ardrone_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,11 +61,15 @@ ARDroneDriver::ARDroneDriver()
set_record_srv = node_handle.advertiseService("ardrone/setrecord", SetRecordCallback);

/* TF Frames */
std::string tf_prefix_key;
private_nh.searchParam("tf_prefix", tf_prefix_key);
private_nh.param(tf_prefix_key, tf_prefix, std::string(""));
private_nh.param<std::string>("drone_frame_id", drone_frame_id, "ardrone_base");
drone_frame_base = drone_frame_id + "_link";
drone_frame_imu = drone_frame_id + "_imu";
drone_frame_front_cam = drone_frame_id + "_frontcam";
drone_frame_bottom_cam = drone_frame_id + "_bottomcam";
drone_frame_base = tf::resolve(tf_prefix, drone_frame_id + "_link");
drone_frame_imu = tf::resolve(tf_prefix, drone_frame_id + "_imu");
drone_frame_front_cam = tf::resolve(tf_prefix, drone_frame_id + "_frontcam");
drone_frame_bottom_cam = tf::resolve(tf_prefix, drone_frame_id + "_bottomcam");
tf_odom.frame_id_ = tf::resolve(tf_prefix, "odom");

if (private_nh.hasParam("root_frame"))
{
Expand Down Expand Up @@ -158,6 +162,7 @@ void ARDroneDriver::run()
{
ROS_WARN("The AR-Drone has a suspicious Firmware number. It usually means the network link is unreliable.");
}
ROS_DEBUG_STREAM("Using " << tf_prefix << " to prefix TF frames.");
}
}
else
Expand Down Expand Up @@ -718,7 +723,6 @@ void ARDroneDriver::PublishOdometry(const navdata_unpacked_t &navdata_raw, const
tf::quaternionMsgToTF(odo_msg.pose.pose.orientation, q);

tf_odom.stamp_ = navdata_receive_time;
tf_odom.frame_id_ = "odom";
tf_odom.child_frame_id_ = drone_frame_base;
tf_odom.setOrigin(t);
tf_odom.setRotation(q);
Expand Down

0 comments on commit c8665fd

Please sign in to comment.