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.

Sign up for GitHub

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

Jump to bottom

Feature set coordinates in the launch file #89

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Feature set coordinates in the launch file #89

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e31dcf4
added output_config param
amastrobera Aug 20, 2018
6b89900
GoToConfig before reset the output (following the pdf guide)
amastrobera Aug 24, 2018
5accd3e
testing orientation and coordinates
amastrobera Aug 29, 2018
fff7663
added only set-coordinates option, in isolation
amastrobera Aug 29, 2018
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
28 changes: 15 additions & 13 deletions launch/xsens_driver.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
<arg name="baudrate" default="0" doc="baudrate of the IMU"/>
<arg name="timeout" default="0.002" doc="timeout for the IMU communication"/>
<arg name="initial_wait" default="0.1" doc="initial wait to allow device to come up"/>
<arg name="set_geo_coordinates" default="51.50,-0.0987,0.0" doc="lat, lon, alt needed to compute the North (default London)"/>
<arg name="frame_id" default="/imu" doc="frame id of the IMU"/>
<arg name="frame_local" default="ENU" doc="desired frame orientation (ENU, NED or NWU)"/>
<arg name="no_rotation_duration" default="0" doc="duration (int in seconds) of the no-rotation calibration procedure"/>
Expand All @@ -12,16 +13,17 @@
<arg name="orientation_covariance_diagonal" default="[0.01745, 0.01745, 0.15708]" doc="Diagonal elements of the orientation covariance matrix"/>

<!-- node -->
<node pkg="xsens_driver" type="mtnode.py" name="xsens_driver" output="screen" >
<param name="device" value="$(arg device)"/>
<param name="baudrate" value="$(arg baudrate)"/>
<param name="timeout" value="$(arg timeout)"/>
<param name="initial_wait" value="$(arg initial_wait)"/>
<param name="frame_id" value="$(arg frame_id)"/>
<param name="frame_local" value="$(arg frame_local)"/>
<param name="no_rotation_duration" value="$(arg no_rotation_duration)"/>
<rosparam param="angular_velocity_covariance_diagonal" subst_value="True">$(arg angular_velocity_covariance_diagonal)</rosparam>
<rosparam param="linear_acceleration_covariance_diagonal" subst_value="True">$(arg linear_acceleration_covariance_diagonal)</rosparam>
<rosparam param="orientation_covariance_diagonal" subst_value="True">$(arg orientation_covariance_diagonal)</rosparam>
</node>
</launch>
<node pkg="xsens_driver" type="mtnode.py" name="xsens_driver" output="screen" >
<param name="device" value="$(arg device)"/>
<param name="baudrate" value="$(arg baudrate)"/>
<param name="timeout" value="$(arg timeout)"/>
<param name="initial_wait" value="$(arg initial_wait)"/>
<param name="set_geo_coordinates" value="$(arg set_geo_coordinates)" />
<param name="frame_id" value="$(arg frame_id)"/>
<param name="frame_local" value="$(arg frame_local)"/>
<param name="no_rotation_duration" value="$(arg no_rotation_duration)"/>
<rosparam param="angular_velocity_covariance_diagonal" subst_value="True">$(arg angular_velocity_covariance_diagonal)</rosparam>
<rosparam param="linear_acceleration_covariance_diagonal" subst_value="True">$(arg linear_acceleration_covariance_diagonal)</rosparam>
<rosparam param="orientation_covariance_diagonal" subst_value="True">$(arg orientation_covariance_diagonal)</rosparam>
</node>
</launch>
10 changes: 10 additions & 0 deletions nodes/mtnode.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ def __init__(self):
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
initial_wait = get_param('~initial_wait', 0.1)
set_geo_coordinates = get_param('~set_geo_coordinates', '51.5,-0.09,0.0')
if device == 'auto':
devs = mtdevice.find_devices(timeout=timeout,
initial_wait=initial_wait)
Expand All @@ -80,6 +81,15 @@ def __init__(self):
self.mt = mtdevice.MTDevice(device, baudrate, timeout,
initial_wait=initial_wait)

if set_geo_coordinates:
coords = set_geo_coordinates.split(",")
if len(coords) != 3:
raise RuntimeError("set_geo_coordinates wrongly formatted: " \
"can only take 3 values comma separated")
lat, lng, alt = float(coords[0]), float(coords[1]), float(coords[2])
rospy.loginfo("Setting Lat, Lng, Alt: %f, %f, %f" % (lat, lng, alt))
self.mt.SetLatLonAlt(lat, lng, alt)

# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
Expand Down