Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
W
wolf_ros_imu
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
mobile_robotics
wolf_projects
wolf_ros
wolf_ros_imu
Commits
f2692e72
Commit
f2692e72
authored
5 years ago
by
Joaquim Casals Buñuel
Browse files
Options
Downloads
Patches
Plain Diff
Rename
parent
85ae44df
No related branches found
No related tags found
2 merge requests
!5
After cmake and const refactor
,
!3
new release
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
CMakeLists.txt
+5
-5
5 additions, 5 deletions
CMakeLists.txt
include/subscriber_imu.h
+7
-7
7 additions, 7 deletions
include/subscriber_imu.h
src/subscriber_imu.cpp
+13
-13
13 additions, 13 deletions
src/subscriber_imu.cpp
with
25 additions
and
25 deletions
CMakeLists.txt
+
5
−
5
View file @
f2692e72
...
@@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
...
@@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
# find_package(Ceres REQUIRED)
# find_package(Ceres REQUIRED)
# find_package(Eigen3 REQUIRED)
# find_package(Eigen3 REQUIRED)
find_package
(
wolf REQUIRED
)
find_package
(
wolf REQUIRED
)
find_package
(
wolf
IMU
REQUIRED
)
find_package
(
wolf
imu
REQUIRED
)
## Uncomment this if the package has a setup.py. This macro ensures
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## modules and global scripts declared therein get installed
...
@@ -125,7 +125,7 @@ include_directories(
...
@@ -125,7 +125,7 @@ include_directories(
include
include
${
EIGEN_INCLUDE_DIRS
}
${
EIGEN_INCLUDE_DIRS
}
${
wolf_INCLUDE_DIRS
}
${
wolf_INCLUDE_DIRS
}
${
wolf
IMU
_INCLUDE_DIRS
}
${
wolf
imu
_INCLUDE_DIRS
}
${
catkin_INCLUDE_DIRS
}
${
catkin_INCLUDE_DIRS
}
${
CERES_INCLUDE_DIRS
}
${
CERES_INCLUDE_DIRS
}
)
)
...
@@ -134,7 +134,7 @@ include_directories(
...
@@ -134,7 +134,7 @@ include_directories(
# add_library(${PROJECT_NAME}
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/wolf_ros.cpp
# src/${PROJECT_NAME}/wolf_ros.cpp
# )
# )
add_library
(
wolf_
subscriber_
IMU
src/
wolf_
subscriber_imu.cpp
)
add_library
(
subscriber_
imu
src/subscriber_imu.cpp
)
## Add cmake target dependencies of the library
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## as an example, code may need to be generated before libraries
...
@@ -158,9 +158,9 @@ add_library(wolf_subscriber_IMU src/wolf_subscriber_imu.cpp)
...
@@ -158,9 +158,9 @@ add_library(wolf_subscriber_IMU src/wolf_subscriber_imu.cpp)
#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
## Specify libraries to link a library or executable target against
## Specify libraries to link a library or executable target against
target_link_libraries
(
wolf_
subscriber_
IMU
target_link_libraries
(
subscriber_
imu
${
wolf_LIBRARIES
}
${
wolf_LIBRARIES
}
${
wolf
IMU
_LIBRARIES
}
${
wolf
imu
_LIBRARIES
}
${
sensor_msgs_LIBRARIES
}
${
sensor_msgs_LIBRARIES
}
)
)
...
...
This diff is collapsed.
Click to expand it.
include/
wolf_
subscriber_imu.h
→
include/subscriber_imu.h
+
7
−
7
View file @
f2692e72
...
@@ -5,8 +5,8 @@
...
@@ -5,8 +5,8 @@
#include
<core/common/wolf.h>
#include
<core/common/wolf.h>
#include
<core/problem/problem.h>
#include
<core/problem/problem.h>
#include
<core/utils/params_server.hpp>
#include
<core/utils/params_server.hpp>
#include
<
IMU
/capture/capture_
IMU
.h>
#include
<
imu
/capture/capture_
imu
.h>
#include
<
IMU
/sensor/sensor_
IMU
.h>
#include
<
imu
/sensor/sensor_
imu
.h>
/**************************
/**************************
* ROS includes *
* ROS includes *
...
@@ -24,21 +24,21 @@
...
@@ -24,21 +24,21 @@
/**************************
/**************************
* WOLF-ROS includes *
* WOLF-ROS includes *
**************************/
**************************/
#include
"
wolf_
subscriber.h"
#include
"subscriber.h"
#include
"subscriber_factory.h"
#include
"subscriber_factory.h"
using
namespace
wolf
;
using
namespace
wolf
;
class
Wolf
SubscriberImu
:
public
Wolf
Subscriber
class
SubscriberImu
:
public
Subscriber
{
{
public:
public:
// Constructor
// Constructor
Wolf
SubscriberImu
(
const
SensorBasePtr
&
sensor_ptr
);
SubscriberImu
(
const
SensorBasePtr
&
sensor_ptr
);
virtual
void
initSubscriber
(
ros
::
NodeHandle
&
nh
,
const
std
::
string
&
topic
);
virtual
void
initSubscriber
(
ros
::
NodeHandle
&
nh
,
const
std
::
string
&
topic
);
void
callback
(
const
sensor_msgs
::
Imu
::
ConstPtr
&
msg
);
void
callback
(
const
sensor_msgs
::
Imu
::
ConstPtr
&
msg
);
static
std
::
shared_ptr
<
Wolf
Subscriber
>
create
(
const
std
::
string
&
_unique_name
,
const
ParamsServer
&
_params
,
const
SensorBasePtr
_sensor_ptr
);
static
std
::
shared_ptr
<
Subscriber
>
create
(
const
std
::
string
&
_unique_name
,
const
ParamsServer
&
_params
,
const
SensorBasePtr
_sensor_ptr
);
};
};
WOLF_REGISTER_SUBSCRIBER
(
Wolf
SubscriberImu
)
WOLF_REGISTER_SUBSCRIBER
(
SubscriberImu
)
This diff is collapsed.
Click to expand it.
src/
wolf_
subscriber_imu.cpp
→
src/subscriber_imu.cpp
+
13
−
13
View file @
f2692e72
#include
"../include/
wolf_
subscriber_imu.h"
#include
"../include/subscriber_imu.h"
using
namespace
wolf
;
using
namespace
wolf
;
// Constructor
// Constructor
Wolf
SubscriberImu
::
Wolf
SubscriberImu
(
const
SensorBasePtr
&
sensor_ptr
)
:
SubscriberImu
::
SubscriberImu
(
const
SensorBasePtr
&
sensor_ptr
)
:
Wolf
Subscriber
(
sensor_ptr
)
Subscriber
(
sensor_ptr
)
{
{
assert
(
std
::
dynamic_pointer_cast
<
SensorI
MU
>
(
sensor_ptr
)
!=
nullptr
);
assert
(
std
::
dynamic_pointer_cast
<
SensorI
mu
>
(
sensor_ptr
)
!=
nullptr
);
}
}
void
Wolf
SubscriberImu
::
initSubscriber
(
ros
::
NodeHandle
&
nh
,
const
std
::
string
&
topic
)
void
SubscriberImu
::
initSubscriber
(
ros
::
NodeHandle
&
nh
,
const
std
::
string
&
topic
)
{
{
sub_
=
nh
.
subscribe
(
topic
,
1
,
&
Wolf
SubscriberImu
::
callback
,
this
);
sub_
=
nh
.
subscribe
(
topic
,
1
,
&
SubscriberImu
::
callback
,
this
);
}
}
void
Wolf
SubscriberImu
::
callback
(
const
sensor_msgs
::
Imu
::
ConstPtr
&
msg
)
void
SubscriberImu
::
callback
(
const
sensor_msgs
::
Imu
::
ConstPtr
&
msg
)
{
{
ROS_INFO
(
"callback I
MU
!"
);
ROS_INFO
(
"callback I
mu
!"
);
Eigen
::
Vector6d
data
;
Eigen
::
Vector6d
data
;
data
<<
msg
->
linear_acceleration
.
x
,
data
<<
msg
->
linear_acceleration
.
x
,
msg
->
linear_acceleration
.
y
,
msg
->
linear_acceleration
.
y
,
...
@@ -30,19 +30,19 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
...
@@ -30,19 +30,19 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
// if covariance not filled
// if covariance not filled
if
(
cov
.
topLeftCorner
<
3
,
3
>
().
isApprox
(
Eigen
::
Matrix3d
::
Zero
(),
1e-9
))
if
(
cov
.
topLeftCorner
<
3
,
3
>
().
isApprox
(
Eigen
::
Matrix3d
::
Zero
(),
1e-9
))
cov
.
topLeftCorner
<
3
,
3
>
()
=
std
::
pow
(
std
::
static_pointer_cast
<
SensorI
MU
>
(
sensor_ptr_
)
->
getAccelNoise
(),
2.0
)
*
Eigen
::
Matrix3d
::
Identity
();
cov
.
topLeftCorner
<
3
,
3
>
()
=
std
::
pow
(
std
::
static_pointer_cast
<
SensorI
mu
>
(
sensor_ptr_
)
->
getAccelNoise
(),
2.0
)
*
Eigen
::
Matrix3d
::
Identity
();
if
(
cov
.
bottomRightCorner
<
3
,
3
>
().
isApprox
(
Eigen
::
Matrix3d
::
Zero
(),
1e-9
))
if
(
cov
.
bottomRightCorner
<
3
,
3
>
().
isApprox
(
Eigen
::
Matrix3d
::
Zero
(),
1e-9
))
cov
.
bottomRightCorner
<
3
,
3
>
()
=
std
::
pow
(
std
::
static_pointer_cast
<
SensorI
MU
>
(
sensor_ptr_
)
->
getGyroNoise
(),
2.0
)
*
Eigen
::
Matrix3d
::
Identity
();
cov
.
bottomRightCorner
<
3
,
3
>
()
=
std
::
pow
(
std
::
static_pointer_cast
<
SensorI
mu
>
(
sensor_ptr_
)
->
getGyroNoise
(),
2.0
)
*
Eigen
::
Matrix3d
::
Identity
();
CaptureI
MU
Ptr
new_capture
=
std
::
make_shared
<
CaptureI
MU
>
(
TimeStamp
(
msg
->
header
.
stamp
.
sec
,
msg
->
header
.
stamp
.
nsec
),
CaptureI
mu
Ptr
new_capture
=
std
::
make_shared
<
CaptureI
mu
>
(
TimeStamp
(
msg
->
header
.
stamp
.
sec
,
msg
->
header
.
stamp
.
nsec
),
sensor_ptr_
,
sensor_ptr_
,
data
,
data
,
cov
);
cov
);
new_capture
->
process
();
new_capture
->
process
();
}
}
std
::
shared_ptr
<
Wolf
Subscriber
>
Wolf
SubscriberImu
::
create
(
const
std
::
string
&
_unique_name
,
const
ParamsServer
&
_params
,
const
SensorBasePtr
_sensor_ptr
)
std
::
shared_ptr
<
Subscriber
>
SubscriberImu
::
create
(
const
std
::
string
&
_unique_name
,
const
ParamsServer
&
_params
,
const
SensorBasePtr
_sensor_ptr
)
{
{
return
std
::
make_shared
<
Wolf
SubscriberImu
>
(
_sensor_ptr
);
return
std
::
make_shared
<
SubscriberImu
>
(
_sensor_ptr
);
}
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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