This class provides access to the Simulink SDK on the AscTec Autopilot's HighLevel Processor (HLP) where the data fusion and position controller are implemented. More...
#include <ssdk_interface.h>
Public Member Functions | |
SSDKInterface (ros::NodeHandle &nh, CommPtr &comm) | |
~SSDKInterface () | |
Private Member Functions | |
void | cbPose (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) |
listens on pose updates via PoseWithCovarianceStamped messages. | |
void | cbSSDKConfig (asctec_hl_interface::SSDKConfig &config, uint32_t level) |
void | cbState (const sensor_fusion_comm::ExtStatePtr &msg) |
this callback sends the full state (position, velocity) to the helicopter and bypasses the Luenberger Observer on the HLP | |
void | processDebugData (uint8_t *buf, uint32_t length) |
void | processStatusData (uint8_t *buf, uint32_t length) |
bool | sendParameters (const asctec_hl_interface::SSDKConfig &config) |
sends parameters to HLP | |
void | sendPoseToAP (const double &x, const double &y, const double &z, const double &yaw, const unsigned char &qual) |
void | tfCallback () |
listens for pose updates via tf | |
Private Attributes | |
CommPtr & | comm_ |
asctec_hl_interface::SSDKConfig | config_ |
SSDKConfigServer * | config_server_ |
ros::Publisher | debug_pub_ |
std::string | frame_id_ |
bool | have_config_ |
ros::NodeHandle | nh_ |
ros::NodeHandle | pnh_ |
ros::Publisher | pose_pub_ |
ros::Subscriber | pose_sub_ |
ros::Subscriber | state_sub_ |
boost::signals::connection | tf_callback_ |
tf::TransformListener | tf_listener_ |
This class provides access to the Simulink SDK on the AscTec Autopilot's HighLevel Processor (HLP) where the data fusion and position controller are implemented.
Datafusion is implemented as a Luenberger observer. For position updates, it listens on either a tf transform or a PoseWithCovarinaceStamped message. Position control needs position, velocity as input, which usually comes from the Luenberger Observer. If datafusion is executed somewhere else than the HLP, position and velocity need to be sent (see SSDKInterface::cbState). For position controller and datafusion, parameters can be modified at runtime through dynamic reconfigure through parameter channels and can be monitored through debug channels. See the package documentation for channel assignments.
Definition at line 61 of file ssdk_interface.h.
SSDKInterface::SSDKInterface | ( | ros::NodeHandle & | nh, |
CommPtr & | comm | ||
) |
Definition at line 36 of file ssdk_interface.cpp.
Definition at line 60 of file ssdk_interface.cpp.
void SSDKInterface::cbPose | ( | const geometry_msgs::PoseWithCovarianceStampedConstPtr & | msg | ) | [private] |
listens on pose updates via PoseWithCovarianceStamped messages.
Definition at line 86 of file ssdk_interface.cpp.
void SSDKInterface::cbSSDKConfig | ( | asctec_hl_interface::SSDKConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 134 of file ssdk_interface.cpp.
void SSDKInterface::cbState | ( | const sensor_fusion_comm::ExtStatePtr & | msg | ) | [private] |
this callback sends the full state (position, velocity) to the helicopter and bypasses the Luenberger Observer on the HLP
Definition at line 170 of file ssdk_interface.cpp.
void SSDKInterface::processDebugData | ( | uint8_t * | buf, |
uint32_t | length | ||
) | [private] |
Definition at line 212 of file ssdk_interface.cpp.
void SSDKInterface::processStatusData | ( | uint8_t * | buf, |
uint32_t | length | ||
) | [private] |
Definition at line 204 of file ssdk_interface.cpp.
bool SSDKInterface::sendParameters | ( | const asctec_hl_interface::SSDKConfig & | config | ) | [private] |
sends parameters to HLP
Definition at line 293 of file ssdk_interface.cpp.
void SSDKInterface::sendPoseToAP | ( | const double & | x, |
const double & | y, | ||
const double & | z, | ||
const double & | yaw, | ||
const unsigned char & | qual | ||
) | [private] |
Definition at line 105 of file ssdk_interface.cpp.
void SSDKInterface::tfCallback | ( | ) | [private] |
listens for pose updates via tf
Definition at line 66 of file ssdk_interface.cpp.
CommPtr& SSDKInterface::comm_ [private] |
Definition at line 67 of file ssdk_interface.h.
asctec_hl_interface::SSDKConfig SSDKInterface::config_ [private] |
Definition at line 77 of file ssdk_interface.h.
SSDKConfigServer* SSDKInterface::config_server_ [private] |
Definition at line 76 of file ssdk_interface.h.
ros::Publisher SSDKInterface::debug_pub_ [private] |
Definition at line 69 of file ssdk_interface.h.
std::string SSDKInterface::frame_id_ [private] |
Definition at line 74 of file ssdk_interface.h.
bool SSDKInterface::have_config_ [private] |
Definition at line 78 of file ssdk_interface.h.
ros::NodeHandle SSDKInterface::nh_ [private] |
Definition at line 65 of file ssdk_interface.h.
ros::NodeHandle SSDKInterface::pnh_ [private] |
Definition at line 66 of file ssdk_interface.h.
ros::Publisher SSDKInterface::pose_pub_ [private] |
Definition at line 70 of file ssdk_interface.h.
ros::Subscriber SSDKInterface::pose_sub_ [private] |
Definition at line 71 of file ssdk_interface.h.
ros::Subscriber SSDKInterface::state_sub_ [private] |
Definition at line 72 of file ssdk_interface.h.
boost::signals::connection SSDKInterface::tf_callback_ [private] |
Definition at line 81 of file ssdk_interface.h.
Definition at line 80 of file ssdk_interface.h.