Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
4
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Switch to GitLab Next
Sign in / Register
Toggle navigation
eskf_odometry_ros
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Locked Files
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Iterations
Merge Requests
0
Merge Requests
0
Requirements
Requirements
List
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Test Cases
Security & Compliance
Security & Compliance
Dependency List
License Compliance
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Package Registry
Container Registry
Analytics
Analytics
CI / CD
Code Review
Insights
Issue
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Angel Santamaria-Navarro
C
code
S
state_estimation
eskf_odometry
eskf_odometry_ros
Commits
d622e3f5
Commit
d622e3f5
authored
Dec 30, 2014
by
adrianamor
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Params initialized. Set quadrotor phase. TODO low-level phase and IMU and PX4 data set
parent
f7f81d31
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
130 additions
and
13 deletions
+130
-13
CMakeLists.txt
CMakeLists.txt
+3
-2
include/eskf_odom_alg.h
include/eskf_odom_alg.h
+18
-0
include/eskf_odom_alg_node.h
include/eskf_odom_alg_node.h
+9
-0
launch/eskf_odom.launch
launch/eskf_odom.launch
+3
-2
package.xml
package.xml
+2
-0
src/eskf_odom_alg.cpp
src/eskf_odom_alg.cpp
+42
-0
src/eskf_odom_alg_node.cpp
src/eskf_odom_alg_node.cpp
+53
-9
No files found.
CMakeLists.txt
View file @
d622e3f5
...
...
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package
(
catkin REQUIRED COMPONENTS iri_base_algorithm px_comm sensor_msgs tf
)
find_package
(
catkin REQUIRED COMPONENTS iri_base_algorithm
asctec_msgs
px_comm sensor_msgs tf
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
...
...
@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm
asctec_msgs
px_comm sensor_msgs tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
...
...
@@ -95,6 +95,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRAR
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
add_dependencies
(
${
PROJECT_NAME
}
asctec_msgs_generate_messages_cpp
)
add_dependencies
(
${
PROJECT_NAME
}
px_comm_generate_messages_cpp
)
add_dependencies
(
${
PROJECT_NAME
}
sensor_msgs_generate_messages_cpp
)
# ********************************************************************
...
...
include/eskf_odom_alg.h
View file @
d622e3f5
...
...
@@ -31,6 +31,8 @@
//include eskf_odom_alg main library
const
double
PX4_MIN_HEIGHT
=
0.31
;
/**
* \brief IRI ROS Specific Driver Class
*
...
...
@@ -137,6 +139,22 @@ class EskfOdomAlgorithm
*/
void
print_ini_params
(
void
);
/**
* \brief Set Observation phase
*
* Sets the observation values depending on the robot phase.
* This step is required due to px4 limited range (0.3m<pz<5m)
* Different values for the sensor:
* -LANDED: sinthetic values (robot not moving).
* -TOFF,LANDING: High covariance in order to reduce update stage
* -FLYING: sensor readings and corresponding covariances.
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void
set_obs_phase
(
const
bool
&
flying
,
const
double
&
gnd_dist
);
/**
* \brief Destructor
*
...
...
include/eskf_odom_alg_node.h
View file @
d622e3f5
...
...
@@ -29,6 +29,7 @@
#include "eskf_odom_alg.h"
// [publisher subscriber headers]
#include <asctec_msgs/LLStatus.h>
#include <px_comm/OpticalFlow.h>
#include <sensor_msgs/Imu.h>
...
...
@@ -49,9 +50,17 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
{
private:
bool
flying_
;
// Quadrotor landed or flying information.
double
gnd_dist_
;
// Ground distance (m) obtained from PX4 optical flow pointing downward.
// [publisher attributes]
// [subscriber attributes]
ros
::
Subscriber
ll_status_subscriber_
;
void
ll_status_callback
(
const
asctec_msgs
::
LLStatus
::
ConstPtr
&
msg
);
pthread_mutex_t
ll_status_mutex_
;
void
ll_status_mutex_enter
(
void
);
void
ll_status_mutex_exit
(
void
);
ros
::
Subscriber
px4_of_subscriber_
;
void
px4_of_callback
(
const
px_comm
::
OpticalFlow
::
ConstPtr
&
msg
);
pthread_mutex_t
px4_of_mutex_
;
...
...
launch/eskf_odom.launch
View file @
d622e3f5
<launch>
<node name="eskf_odom" pkg="eskf_odom" type="eskf_odom" output="screen">
<remap from="imu" to="/imu" />
<remap from="px4_of" to="/px4_of" />
<remap from="~imu" to="/imu" />
<remap from="~px4_of" to="/px4flow/opt_flow" />
<remap from="~ll_status" to="/asctec/LL_STATUS" />
<rosparam file="$(find eskf_odom)/launch/params.yaml"/>
</node>
...
...
package.xml
View file @
d622e3f5
...
...
@@ -9,12 +9,14 @@
<buildtool_depend>
catkin
</buildtool_depend>
<build_depend>
iri_base_algorithm
</build_depend>
<build_depend>
asctec_msgs
</build_depend>
<build_depend>
px_comm
</build_depend>
<build_depend>
sensor_msgs
</build_depend>
<build_depend>
tf
</build_depend>
<build_depend>
eskf_odometry
</build_depend>
<run_depend>
iri_base_algorithm
</run_depend>
<run_depend>
asctec_msgs
</run_depend>
<run_depend>
px_comm
</run_depend>
<run_depend>
tf
</run_depend>
<run_depend>
sensor_msgs
</run_depend>
...
...
src/eskf_odom_alg.cpp
View file @
d622e3f5
...
...
@@ -3,6 +3,10 @@
EskfOdomAlgorithm
::
EskfOdomAlgorithm
(
void
)
{
pthread_mutex_init
(
&
this
->
access_
,
NULL
);
// Set initial robot phase as landed
this
->
filter_
.
set_robot_phase
(
Robot
::
LANDED
);
this
->
filter_
.
print_robot_phase
();
}
EskfOdomAlgorithm
::~
EskfOdomAlgorithm
(
void
)
...
...
@@ -28,4 +32,42 @@ void EskfOdomAlgorithm::set_init_params(const params& f_params, const VectorXd&
void
EskfOdomAlgorithm
::
print_ini_params
()
{
this
->
filter_
.
print_ini_params
();
}
void
EskfOdomAlgorithm
::
set_obs_phase
(
const
bool
&
flying
,
const
double
&
gnd_dist
)
{
Robot
::
kinton_phases
phase
;
this
->
filter_
.
get_robot_phase
(
phase
);
switch
(
phase
)
{
case
Robot
::
LANDED
:
if
(
flying
){
this
->
filter_
.
set_robot_phase
(
Robot
::
TOFF
);
this
->
filter_
.
print_robot_phase
();
}
break
;
case
Robot
::
TOFF
:
if
(
gnd_dist
>
PX4_MIN_HEIGHT
){
this
->
filter_
.
set_robot_phase
(
Robot
::
FLYING
);
this
->
filter_
.
print_robot_phase
();
}
break
;
case
Robot
::
FLYING
:
if
(
gnd_dist
<
PX4_MIN_HEIGHT
){
this
->
filter_
.
set_robot_phase
(
Robot
::
LANDING
);
this
->
filter_
.
print_robot_phase
();
}
break
;
case
Robot
::
LANDING
:
if
(
!
flying
){
this
->
filter_
.
set_robot_phase
(
Robot
::
LANDED
);
this
->
filter_
.
print_robot_phase
();
}
else
if
(
gnd_dist
>
PX4_MIN_HEIGHT
){
this
->
filter_
.
set_robot_phase
(
Robot
::
FLYING
);
this
->
filter_
.
print_robot_phase
();
}
break
;
}
}
\ No newline at end of file
src/eskf_odom_alg_node.cpp
View file @
d622e3f5
#include "eskf_odom_alg_node.h"
//TODO: Quadrotor phases (taking-off, flying, landing, landed)
//this means adding subscription to low-level quadrotor driver
//TODO: LOW-LEVEL LIBRARY
// Quadrotor phases (taking-off, flying, landing, landed)
// this means adding subscription to low-level quadrotor driver
//TODO: NODE
// SET IMU and PX4 data
using
namespace
std
;
using
namespace
Eigen
;
...
...
@@ -12,9 +16,16 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// Initialize ROBOT phase
this
->
flying_
=
false
;
this
->
gnd_dist_
=
PX4_MIN_HEIGHT
;
// a little bit more than the minimum distance PX4 can detect (0.3m)
// [init publishers]
// [init subscribers]
this
->
ll_status_subscriber_
=
this
->
public_node_handle_
.
subscribe
(
"ll_status"
,
1
,
&
EskfOdomAlgNode
::
ll_status_callback
,
this
);
pthread_mutex_init
(
&
this
->
ll_status_mutex_
,
NULL
);
this
->
px4_of_subscriber_
=
this
->
public_node_handle_
.
subscribe
(
"px4_of"
,
1
,
&
EskfOdomAlgNode
::
px4_of_callback
,
this
);
pthread_mutex_init
(
&
this
->
px4_of_mutex_
,
NULL
);
...
...
@@ -36,6 +47,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
EskfOdomAlgNode
::~
EskfOdomAlgNode
(
void
)
{
// [free dynamic memory]
pthread_mutex_destroy
(
&
this
->
ll_status_mutex_
);
pthread_mutex_destroy
(
&
this
->
px4_of_mutex_
);
pthread_mutex_destroy
(
&
this
->
imu_mutex_
);
}
...
...
@@ -120,22 +132,56 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
// Set Quadrotor State
this
->
ll_status_mutex_enter
();
this
->
px4_of_mutex_enter
();
this
->
alg_
.
set_obs_phase
(
this
->
flying_
,
this
->
gnd_dist_
);
this
->
ll_status_mutex_exit
();
this
->
px4_of_mutex_exit
();
}
/* [subscriber callbacks] */
void
EskfOdomAlgNode
::
ll_status_callback
(
const
asctec_msgs
::
LLStatus
::
ConstPtr
&
msg
)
{
ROS_DEBUG
(
"EskfOdomAlgNode::ll_status_callback: New Message Received"
);
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this
->
ll_status_mutex_enter
();
this
->
flying_
=
msg
->
flying
;
//unlock previously blocked shared variables
//this->alg_.unlock();
this
->
ll_status_mutex_exit
();
}
void
EskfOdomAlgNode
::
ll_status_mutex_enter
(
void
)
{
pthread_mutex_lock
(
&
this
->
ll_status_mutex_
);
}
void
EskfOdomAlgNode
::
ll_status_mutex_exit
(
void
)
{
pthread_mutex_unlock
(
&
this
->
ll_status_mutex_
);
}
void
EskfOdomAlgNode
::
px4_of_callback
(
const
px_comm
::
OpticalFlow
::
ConstPtr
&
msg
)
{
ROS_
INFO
(
"EskfOdomAlgNode::px4_of_callback: New Message Received"
);
ROS_
DEBUG
(
"EskfOdomAlgNode::px4_of_callback: New Message Received"
);
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//
this->px4_of_mutex_enter();
this
->
px4_of_mutex_enter
();
//std::cout << msg->data << std::endl;
this
->
gnd_dist_
=
msg
->
ground_distance
;
//unlock previously blocked shared variables
//this->alg_.unlock();
//
this->px4_of_mutex_exit();
this
->
px4_of_mutex_exit
();
}
void
EskfOdomAlgNode
::
px4_of_mutex_enter
(
void
)
...
...
@@ -149,14 +195,12 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
}
void
EskfOdomAlgNode
::
imu_callback
(
const
sensor_msgs
::
Imu
::
ConstPtr
&
msg
)
{
ROS_
INFO
(
"EskfOdomAlgNode::imu_callback: New Message Received"
);
ROS_
DEBUG
(
"EskfOdomAlgNode::imu_callback: New Message Received"
);
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//this->imu_mutex_enter();
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
//this->imu_mutex_exit();
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment