#include <IdentificationNode.hpp>
Public Member Functions | |
IdentificationNode () | |
void | onInit () |
Private Types | |
enum | { x = 0, y, z, roll, pitch, yaw, altitude, u, v, w, measNum } |
enum | { X = 0, Y, Z, K, M, N } |
enum | { alpha = 0, beta, betaa } |
typedef navcon_msgs::DOFIdentificationAction | Action |
typedef actionlib::SimpleActionServer < Action > | ActionServer |
typedef boost::shared_ptr < ActionServer > | ActionServerPtr |
typedef navcon_msgs::DOFIdentificationGoal | Goal |
typedef Eigen::Matrix< double, measNum, 1 > | MeasVec |
typedef navcon_msgs::DOFIdentificationResult | Result |
Private Member Functions | |
void | doIdentification (const Goal::ConstPtr &goal) |
void | onMeasurement (const auv_msgs::NavSts::ConstPtr &meas) |
void | setTau (int elem, double value) |
Private Attributes | |
ActionServerPtr | aserver |
double | cumulative_error |
boost::thread | identrunner |
bool | integrateUV |
ros::Subscriber | meas |
boost::mutex | measmux |
MeasVec | measurements |
ros::Publisher | stateOut |
ros::Publisher | tauOut |
bool | useUV |
bool | useW |
The class contains the implementation of the SO identification algorithm in the ROS framework.
Set feedback value in ident action.
Make SOIdentification more user friendly C++ class.
Make SOIdentification more simple/ C implementable for uC.
Definition at line 57 of file IdentificationNode.hpp.
typedef navcon_msgs::DOFIdentificationAction labust::control::IdentificationNode::Action [private] |
Definition at line 63 of file IdentificationNode.hpp.
typedef actionlib::SimpleActionServer<Action> labust::control::IdentificationNode::ActionServer [private] |
Definition at line 64 of file IdentificationNode.hpp.
typedef boost::shared_ptr<ActionServer> labust::control::IdentificationNode::ActionServerPtr [private] |
Definition at line 65 of file IdentificationNode.hpp.
typedef navcon_msgs::DOFIdentificationGoal labust::control::IdentificationNode::Goal [private] |
Definition at line 66 of file IdentificationNode.hpp.
typedef Eigen::Matrix<double, measNum,1> labust::control::IdentificationNode::MeasVec [private] |
Definition at line 69 of file IdentificationNode.hpp.
typedef navcon_msgs::DOFIdentificationResult labust::control::IdentificationNode::Result [private] |
Definition at line 67 of file IdentificationNode.hpp.
anonymous enum [private] |
Definition at line 59 of file IdentificationNode.hpp.
anonymous enum [private] |
Definition at line 60 of file IdentificationNode.hpp.
anonymous enum [private] |
Definition at line 61 of file IdentificationNode.hpp.
Main constructor
Definition at line 49 of file IdentificationNode.cpp.
void IdentificationNode::doIdentification | ( | const Goal::ConstPtr & | goal | ) | [private] |
Execute the identification loop.
Definition at line 108 of file IdentificationNode.cpp.
void IdentificationNode::onInit | ( | ) |
Initialize and setup controller.
Definition at line 58 of file IdentificationNode.cpp.
void IdentificationNode::onMeasurement | ( | const auv_msgs::NavSts::ConstPtr & | meas | ) | [private] |
Handle incoming estimates message.
Definition at line 77 of file IdentificationNode.cpp.
void IdentificationNode::setTau | ( | int | elem, |
double | value | ||
) | [private] |
Publish the force and torque values.
Definition at line 241 of file IdentificationNode.cpp.
The identification action server.
Definition at line 106 of file IdentificationNode.hpp.
double labust::control::IdentificationNode::cumulative_error [private] |
Cumulative error.
Definition at line 137 of file IdentificationNode.hpp.
boost::thread labust::control::IdentificationNode::identrunner [private] |
The identification thread.
Definition at line 110 of file IdentificationNode.hpp.
bool labust::control::IdentificationNode::integrateUV [private] |
Integration based x,y. The x,y measurements will be calculated by integration of forward and lateral speed measurements.
Definition at line 121 of file IdentificationNode.hpp.
The subscribed topics.
Definition at line 102 of file IdentificationNode.hpp.
boost::mutex labust::control::IdentificationNode::measmux [private] |
The measurement mutex.
Definition at line 141 of file IdentificationNode.hpp.
The measurement vector.
Definition at line 114 of file IdentificationNode.hpp.
Definition at line 98 of file IdentificationNode.hpp.
The publisher of the TAU message.
Definition at line 98 of file IdentificationNode.hpp.
bool labust::control::IdentificationNode::useUV [private] |
Use surge-sway error. The difference between the desired speed and measured will be integrated as the error to the identification.
Definition at line 127 of file IdentificationNode.hpp.
bool labust::control::IdentificationNode::useW [private] |
Use heave error. The difference between the desired heave and measured will be integrated as the error to the identification.
Definition at line 133 of file IdentificationNode.hpp.