Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
imu
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Container Registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
CdF
modules
imu
Commits
bfaec511
Commit
bfaec511
authored
4 years ago
by
Paul D'Angio
Browse files
Options
Downloads
Patches
Plain Diff
updated to new imu message types from sensor_msgs_ext
parent
7628a073
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/ros_node.cpp
+9
-9
9 additions, 9 deletions
src/ros_node.cpp
with
9 additions
and
9 deletions
src/ros_node.cpp
+
9
−
9
View file @
bfaec511
#include
"ros_node.h"
#include
"ros_node.h"
#include
<sensor_msgs_ext/acceler
ometer
.h>
#include
<sensor_msgs_ext/acceler
ation
.h>
#include
<sensor_msgs_ext/
gyroscope
.h>
#include
<sensor_msgs_ext/
angular_velocity
.h>
#include
<sensor_msgs_ext/magnet
ometer
.h>
#include
<sensor_msgs_ext/magnet
ic_field
.h>
#include
<sensor_msgs_ext/temperature.h>
#include
<sensor_msgs_ext/temperature.h>
#include
<cmath>
#include
<cmath>
...
@@ -31,9 +31,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
...
@@ -31,9 +31,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Set up publishers.
// Set up publishers.
ros_node
::
m_publisher_accelerometer
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
acceler
ometer
>
(
"imu/accelerometer"
,
1
);
ros_node
::
m_publisher_accelerometer
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
acceler
ation
>
(
"imu/accelerometer"
,
1
);
ros_node
::
m_publisher_gyroscope
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
gyroscope
>
(
"imu/gyroscope"
,
1
);
ros_node
::
m_publisher_gyroscope
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
angular_velocity
>
(
"imu/gyroscope"
,
1
);
ros_node
::
m_publisher_magnetometer
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
magnet
ometer
>
(
"imu/magnetometer"
,
1
);
ros_node
::
m_publisher_magnetometer
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
magnet
ic_field
>
(
"imu/magnetometer"
,
1
);
ros_node
::
m_publisher_temperature
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
temperature
>
(
"imu/temperature"
,
1
);
ros_node
::
m_publisher_temperature
=
ros_node
::
m_node
->
advertise
<
sensor_msgs_ext
::
temperature
>
(
"imu/temperature"
,
1
);
// Initialize the driver and set parameters.
// Initialize the driver and set parameters.
...
@@ -92,7 +92,7 @@ void ros_node::deinitialize_driver()
...
@@ -92,7 +92,7 @@ void ros_node::deinitialize_driver()
void
ros_node
::
data_callback
(
driver
::
data
data
)
void
ros_node
::
data_callback
(
driver
::
data
data
)
{
{
// Create accelerometer message.
// Create accelerometer message.
sensor_msgs_ext
::
acceler
ometer
message_accel
;
sensor_msgs_ext
::
acceler
ation
message_accel
;
// Set accelerations (convert from g's to m/s^2)
// Set accelerations (convert from g's to m/s^2)
message_accel
.
x
=
static_cast
<
double
>
(
data
.
accel_x
)
*
9.80665
;
message_accel
.
x
=
static_cast
<
double
>
(
data
.
accel_x
)
*
9.80665
;
message_accel
.
y
=
static_cast
<
double
>
(
data
.
accel_y
)
*
9.80665
;
message_accel
.
y
=
static_cast
<
double
>
(
data
.
accel_y
)
*
9.80665
;
...
@@ -101,7 +101,7 @@ void ros_node::data_callback(driver::data data)
...
@@ -101,7 +101,7 @@ void ros_node::data_callback(driver::data data)
ros_node
::
m_publisher_accelerometer
.
publish
(
message_accel
);
ros_node
::
m_publisher_accelerometer
.
publish
(
message_accel
);
// Create gyroscope message.
// Create gyroscope message.
sensor_msgs_ext
::
gyroscope
message_gyro
;
sensor_msgs_ext
::
angular_velocity
message_gyro
;
// Set rotation rates (convert from deg/sec to rad/sec)
// Set rotation rates (convert from deg/sec to rad/sec)
message_gyro
.
x
=
static_cast
<
double
>
(
data
.
gyro_x
)
*
M_PI
/
180.0
;
message_gyro
.
x
=
static_cast
<
double
>
(
data
.
gyro_x
)
*
M_PI
/
180.0
;
message_gyro
.
y
=
static_cast
<
double
>
(
data
.
gyro_y
)
*
M_PI
/
180.0
;
message_gyro
.
y
=
static_cast
<
double
>
(
data
.
gyro_y
)
*
M_PI
/
180.0
;
...
@@ -113,7 +113,7 @@ void ros_node::data_callback(driver::data data)
...
@@ -113,7 +113,7 @@ void ros_node::data_callback(driver::data data)
if
(
isnan
(
data
.
magneto_x
)
==
false
)
if
(
isnan
(
data
.
magneto_x
)
==
false
)
{
{
// Create magneto message.
// Create magneto message.
sensor_msgs_ext
::
magnet
ometer
message_mag
;
sensor_msgs_ext
::
magnet
ic_field
message_mag
;
// Fill magnetic field strengths (convert from uT to T)
// Fill magnetic field strengths (convert from uT to T)
message_mag
.
x
=
static_cast
<
double
>
(
data
.
magneto_x
)
*
0.000001
;
message_mag
.
x
=
static_cast
<
double
>
(
data
.
magneto_x
)
*
0.000001
;
message_mag
.
y
=
static_cast
<
double
>
(
data
.
magneto_y
)
*
0.000001
;
message_mag
.
y
=
static_cast
<
double
>
(
data
.
magneto_y
)
*
0.000001
;
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment