#include <qnode.hpp>
Public Types | |
enum | LogLevel { Debug = 0, Info = 1, Warn = 2, Error = 3, Fatal = 4 } |
Public Slots | |
void | getJointControlModule () |
void | getJointPose (std::string joint_name) |
void | getKinematicsPose (std::string group_name) |
void | getKinematicsPoseCallback (const geometry_msgs::Pose::ConstPtr &msg) |
void | setCurrentControlUI (int mode) |
Signals | |
void | loggingUpdated () |
void | rosShutdown () |
void | updateCurrJoint (double value) |
void | updateCurrOri (double x, double y, double z, double w) |
void | updateCurrPos (double x, double y, double z) |
void | updateDemoPoint (const geometry_msgs::Point point) |
void | updateDemoPose (const geometry_msgs::Pose pose) |
void | updateHeadJointsAngle (double pan, double tilt) |
void | updatePresentJointControlModules (std::vector< int > mode) |
Public Member Functions | |
void | assembleLidar () |
void | clearFootsteps () |
void | clearInteractiveMarker () |
void | clearLog () |
void | clearUsingModule () |
void | enableControlModule (const std::string &mode) |
bool | getIDFromJointName (const std::string &joint_name, int &id) |
bool | getIDJointNameFromIndex (const int &index, int &id, std::string &joint_name) |
void | getInteractiveMarkerPose () |
bool | getJointNameFromID (const int &id, std::string &joint_name) |
int | getJointTableSize () |
int | getModuleIndex (const std::string &mode_name) |
std::string | getModuleName (const int &index) |
int | getModuleTableSize () |
bool | init () |
void | initFTCommand (std::string command) |
bool | isUsingModule (const std::string &module_name) |
void | kickDemo (const std::string &kick_foot) |
void | log (const LogLevel &level, const std::string &msg, std::string sender="Demo") |
QStringListModel * | loggingModel () |
void | makeFootstepUsingPlanner () |
void | makeFootstepUsingPlanner (const geometry_msgs::Pose &target_foot_pose) |
void | makeInteractiveMarker (const geometry_msgs::Pose &marker_pose) |
void | manipulationDemo (const int &index) |
void | moveInitPose () |
void | playMotion (int motion_index, bool to_action_script=true) |
QNodeThor3 (int argc, char **argv) | |
void | run () |
void | sendDestJointMsg (thormang3_manipulation_module_msgs::JointPose msg) |
void | sendGripperPosition (sensor_msgs::JointState msg) |
void | sendIkMsg (thormang3_manipulation_module_msgs::KinematicsPose msg) |
void | sendInitPoseMsg (std_msgs::String msg) |
bool | setFeedBackGain () |
void | setHeadJoint (double pan, double tilt) |
void | setWalkingBalance (bool on_command) |
void | setWalkingBalanceParam (const double &gyro_gain, const double &ft_gain_ratio, const double &imu_time_const, const double &ft_time_const) |
void | setWalkingCommand (thormang3_foot_step_generator::FootStepCommand msg) |
void | setWalkingFootsteps () |
void | updateInteractiveMarker (const geometry_msgs::Pose &pose) |
void | visualizePreviewFootsteps (bool clear) |
virtual | ~QNodeThor3 () |
Public Attributes | |
std::map< int, std::string > | module_table_ |
std::map< int, std::string > | motion_table_ |
std::string | package_name_ |
Private Types | |
enum | Control_Index { MODE_UI = 0, WALKING_UI = 1, MANIPULATION_UI = 2, HEAD_CONTROL_UI = 3, MOTION_UI = 4, DEMO_UI = 5 } |
Private Member Functions | |
void | initFTFootCallback (const thormang3_feet_ft_module_msgs::BothWrench::ConstPtr &msg) |
void | interactiveMarkerFeedback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
bool | loadBalanceParameterFromYaml () |
bool | loadFeedbackGainFromYaml () |
void | parseJointNameFromYaml (const std::string &path) |
void | parseMotionMapFromYaml (const std::string &path) |
void | pointStampedCallback (const geometry_msgs::PointStamped::ConstPtr &msg) |
void | poseCallback (const geometry_msgs::Pose::ConstPtr &msg) |
void | refreshCurrentJointControlCallback (const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) |
void | setBalanceParameter () |
void | statusMsgCallback (const robotis_controller_msgs::StatusMsg::ConstPtr &msg) |
void | turnOffBalance () |
void | turnOnBalance () |
void | updateHeadJointStatesCallback (const sensor_msgs::JointState::ConstPtr &msg) |
Static Private Attributes | |
static const double | DEGREE2RADIAN = M_PI / 180.0 |
static const double | RADIAN2DEGREE = 180.0 / M_PI |
|
private |
thormang3_demo::QNodeThor3::QNodeThor3 | ( | int | argc, |
char ** | argv | ||
) |
void thormang3_demo::QNodeThor3::clearInteractiveMarker | ( | ) |
void thormang3_demo::QNodeThor3::enableControlModule | ( | const std::string & | mode | ) |
bool thormang3_demo::QNodeThor3::getIDFromJointName | ( | const std::string & | joint_name, |
int & | id | ||
) |
bool thormang3_demo::QNodeThor3::getIDJointNameFromIndex | ( | const int & | index, |
int & | id, | ||
std::string & | joint_name | ||
) |
void thormang3_demo::QNodeThor3::getInteractiveMarkerPose | ( | ) |
|
slot |
bool thormang3_demo::QNodeThor3::getJointNameFromID | ( | const int & | id, |
std::string & | joint_name | ||
) |
|
slot |
|
slot |
|
slot |
int thormang3_demo::QNodeThor3::getModuleIndex | ( | const std::string & | mode_name | ) |
std::string thormang3_demo::QNodeThor3::getModuleName | ( | const int & | index | ) |
void thormang3_demo::QNodeThor3::initFTCommand | ( | std::string | command | ) |
|
private |
|
private |
bool thormang3_demo::QNodeThor3::isUsingModule | ( | const std::string & | module_name | ) |
void thormang3_demo::QNodeThor3::kickDemo | ( | const std::string & | kick_foot | ) |
|
private |
|
private |
void thormang3_demo::QNodeThor3::log | ( | const LogLevel & | level, |
const std::string & | msg, | ||
std::string | sender = "Demo" |
||
) |
|
inline |
|
signal |
void thormang3_demo::QNodeThor3::makeFootstepUsingPlanner | ( | ) |
void thormang3_demo::QNodeThor3::makeFootstepUsingPlanner | ( | const geometry_msgs::Pose & | target_foot_pose | ) |
void thormang3_demo::QNodeThor3::makeInteractiveMarker | ( | const geometry_msgs::Pose & | marker_pose | ) |
void thormang3_demo::QNodeThor3::manipulationDemo | ( | const int & | index | ) |
|
private |
|
private |
void thormang3_demo::QNodeThor3::playMotion | ( | int | motion_index, |
bool | to_action_script = true |
||
) |
|
private |
|
private |
|
private |
|
signal |
void thormang3_demo::QNodeThor3::sendDestJointMsg | ( | thormang3_manipulation_module_msgs::JointPose | msg | ) |
void thormang3_demo::QNodeThor3::sendGripperPosition | ( | sensor_msgs::JointState | msg | ) |
void thormang3_demo::QNodeThor3::sendIkMsg | ( | thormang3_manipulation_module_msgs::KinematicsPose | msg | ) |
void thormang3_demo::QNodeThor3::sendInitPoseMsg | ( | std_msgs::String | msg | ) |
|
private |
|
slot |
void thormang3_demo::QNodeThor3::setHeadJoint | ( | double | pan, |
double | tilt | ||
) |
void thormang3_demo::QNodeThor3::setWalkingBalance | ( | bool | on_command | ) |
void thormang3_demo::QNodeThor3::setWalkingBalanceParam | ( | const double & | gyro_gain, |
const double & | ft_gain_ratio, | ||
const double & | imu_time_const, | ||
const double & | ft_time_const | ||
) |
void thormang3_demo::QNodeThor3::setWalkingCommand | ( | thormang3_foot_step_generator::FootStepCommand | msg | ) |
|
private |
|
private |
|
private |
|
signal |
|
signal |
|
signal |
|
signal |
|
signal |
|
signal |
|
private |
void thormang3_demo::QNodeThor3::updateInteractiveMarker | ( | const geometry_msgs::Pose & | pose | ) |
|
signal |
void thormang3_demo::QNodeThor3::visualizePreviewFootsteps | ( | bool | clear | ) |
|
private |
|
private |
|
private |
|
private |
|
private |
|
staticprivate |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
std::map<int, std::string> thormang3_demo::QNodeThor3::module_table_ |
|
private |
|
private |
std::map<int, std::string> thormang3_demo::QNodeThor3::motion_table_ |
|
private |
|
private |
|
private |
|
private |
|
private |
|
staticprivate |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |