Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove tf prefix #252

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 8 additions & 5 deletions launch/swarmie.launch
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
<launch>

<param name="tf_prefix" value="$(arg name)" />

<node name="$(arg name)_BASE2CAM" pkg="tf" type="static_transform_publisher" args="0.12 -0.03 0.195 -1.57 0 -2.22 $(arg name)/base_link $(arg name)/camera_link 100" />
<node name="$(arg name)_DIAGNOSTICS" pkg="diagnostics" type="diagnostics" args="$(arg name)" />
<node name="$(arg name)_SBRIDGE" pkg="sbridge" type="sbridge" args="$(arg name)" />
Expand All @@ -11,7 +9,7 @@

<param name="magnetic_declination_radians" value="0.0"/>
<param name="yaw_offset" value="1.57079632679"/>
<param name="world_frame" value="map"/>
<param name="world_frame" value="$(arg name)/map"/>
<param name="frequency" value="10"/>

<remap from="/imu/data" to="/$(arg name)/imu" />
Expand All @@ -27,7 +25,9 @@
<param name="odom0" value="/$(arg name)/odom" />
<param name="imu0" value="/$(arg name)/imu" />
<param name="two_d_mode" value="true" />
<param name="world_frame" value="odom" />
<param name="world_frame" value="$(arg name)/odom" />
<param name="odom_frame" value="$(arg name)/odom" />
<param name="base_link_frame" value="$(arg name)/base_link" />
<param name="frequency" value="10" />

<rosparam param="odom0_config">[false, false, false,
Expand Down Expand Up @@ -79,7 +79,10 @@
<param name="odom1" value="/$(arg name)/odom/filtered" />
<param name="imu0" value="/$(arg name)/imu" />
<param name="two_d_mode" value="true" />
<param name="world_frame" value="map" />
<param name="world_frame" value="$(arg name)/map" />
<param name="map_frame" value="$(arg name)/map" />
<param name="odom_frame" value="$(arg name)/odom" />
<param name="base_link_frame" value="$(arg name)/base_link" />
<param name="frequency" value="10" />

<rosparam param="odom0_config">[true, true, false,
Expand Down
11 changes: 3 additions & 8 deletions misc/rover_onboard_node_launch.sh
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,6 @@ else
fi


#Set prefix to fully qualify transforms for each robot
echo "set prefix to fully qualify transforms for each robot: $HOSTNAME"
rosparam set tf_prefix $HOSTNAME


#Function to lookup correct path for a given device
echo "findDevicePath() for usb devices (arduino, camera, etc)"
findDevicePath() {
Expand Down Expand Up @@ -109,13 +104,13 @@ else
nohup > logs/$HOSTNAME"_ublox_log.txt" rosrun ublox_gps ublox_gps __name:=$HOSTNAME\_UBLOX /$HOSTNAME\_UBLOX/fix:=/$HOSTNAME/fix /$HOSTNAME\_UBLOX/fix_velocity:=/$HOSTNAME/fix_velocity /$HOSTNAME\_UBLOX/navposllh:=/$HOSTNAME/navposllh /$HOSTNAME\_UBLOX/navsol:=/$HOSTNAME/navsol /$HOSTNAME\_UBLOX/navstatus:=/$HOSTNAME/navstatus /$HOSTNAME\_UBLOX/navvelned:=/$HOSTNAME/navvelned _device:=/dev/$gpsDevicePath _frame_id:=$HOSTNAME/base_link &
fi

nohup > logs/$HOSTNAME"_localization_navsat_log.txt" rosrun robot_localization navsat_transform_node __name:=$HOSTNAME\_NAVSAT _world_frame:=map _frequency:=10 _magnetic_declination_radians:=0.1530654 _yaw_offset:=0 /imu/data:=/$HOSTNAME/imu /gps/fix:=/$HOSTNAME/fix /odometry/filtered:=/$HOSTNAME/odom/ekf /odometry/gps:=/$HOSTNAME/odom/navsat &
nohup > logs/$HOSTNAME"_localization_navsat_log.txt" rosrun robot_localization navsat_transform_node __name:=$HOSTNAME\_NAVSAT _world_frame:=$HOSTNAME/map _frequency:=10 _magnetic_declination_radians:=0.1530654 _yaw_offset:=0 /imu/data:=/$HOSTNAME/imu /gps/fix:=/$HOSTNAME/fix /odometry/filtered:=/$HOSTNAME/odom/ekf /odometry/gps:=/$HOSTNAME/odom/navsat &

rosparam set /$HOSTNAME\_ODOM/odom0 /$HOSTNAME/odom
rosparam set /$HOSTNAME\_ODOM/imu0 /$HOSTNAME/imu
rosparam set /$HOSTNAME\_ODOM/odom0_config [false,false,false,false,false,false,true,false,false,false,false,true,false,false,false]
rosparam set /$HOSTNAME\_ODOM/imu0_config [false,false,false,false,false,true,false,false,false,false,false,true,true,false,false]
nohup > logs/$HOSTNAME"_odom_EKF_log.txt" rosrun robot_localization ekf_localization_node _two_d_mode:=true _world_frame:=odom _frequency:=10 __name:=$HOSTNAME\_ODOM /odometry/filtered:=/$HOSTNAME/odom/filtered &
nohup > logs/$HOSTNAME"_odom_EKF_log.txt" rosrun robot_localization ekf_localization_node _two_d_mode:=true _world_frame:=$HOSTNAME/odom _odom_frame:=$HOSTNAME/odom _base_link_frame:=$HOSTNAME/base_link _frequency:=10 __name:=$HOSTNAME\_ODOM /odometry/filtered:=/$HOSTNAME/odom/filtered &

rosparam set /$HOSTNAME\_MAP/odom0 /$HOSTNAME/odom/navsat
rosparam set /$HOSTNAME\_MAP/odom1 /$HOSTNAME/odom/filtered
Expand Down Expand Up @@ -157,7 +152,7 @@ rosparam set $HOSTNAME\_MAP/process_noise_covariance "[0.005, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, /
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1]"

nohup > logs/$HOSTNAME"_map_EKF_log.txt" rosrun robot_localization ekf_localization_node _two_d_mode:=true _world_frame:=map _frequency:=10 __name:=$HOSTNAME\_MAP /odometry/filtered:=/$HOSTNAME/odom/ekf &
nohup > logs/$HOSTNAME"_map_EKF_log.txt" rosrun robot_localization ekf_localization_node _two_d_mode:=true _world_frame:=$HOSTNAME/map _map_frame:=$HOSTNAME/map _odom_frame:=$HOSTNAME/odom _base_link_frame:=$HOSTNAME/base_link _frequency:=10 __name:=$HOSTNAME\_MAP /odometry/filtered:=/$HOSTNAME/odom/ekf &

throttle()
{
Expand Down