#include <marty_core.hpp>
|
| void | accelCB (const marty_msgs::Accelerometer::ConstPtr &msg) |
| | Accelerometer data callback. Accelerometer data used to detect falls and for walking odometry. More...
|
| |
| void | battCB (const std_msgs::Float32::ConstPtr &msg) |
| |
| void | gpioCB (const marty_msgs::GPIOs::ConstPtr &msg) |
| |
| void | jointCB (const marty_msgs::ServoMsg::ConstPtr &msg) |
| |
| void | jointsCB (const marty_msgs::ServoMsgArray::ConstPtr &msg) |
| |
| void | kfCB (const std_msgs::String::ConstPtr &msg) |
| |
| void | sendKeyframes (marty_msgs::KeyframeArray k_array) |
| |
| bool | setFallDetector (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) |
| |
| void | setupJointStates () |
| |
| void | setupOdometry () |
| |
| void | soundCB (const std_msgs::String::ConstPtr &msg) |
| |
| void | tfCB (const ros::TimerEvent &e) |
| | Publishes TF and joint states on a timer callback. More...
|
| |
| void | updateJointState (marty_msgs::ServoMsg servo) |
| | Updates the joint state positions of every joint given servo cmds. More...
|
| |
| void | updateOdom () |
| | This function updates the Odometry given Marty's motion and sensors. More...
|
| |
Definition at line 57 of file marty_core.hpp.
| MartyCore::MartyCore |
( |
ros::NodeHandle & |
nh | ) |
|
| MartyCore::~MartyCore |
( |
| ) |
|
| void MartyCore::accelCB |
( |
const marty_msgs::Accelerometer::ConstPtr & |
msg | ) |
|
|
private |
Accelerometer data callback. Accelerometer data used to detect falls and for walking odometry.
If accelerometer y (TODO change to z) is smaller than the fall threshold, a warning is printed and the motors are disabled. When Marty returns to upright position motors are re-enabled. Accelerometer data is used for estimating roll/pitch/yaw. Due to gimbal lock and evil quaternions yaw is not useful, but the others are published.
- Parameters
-
| msg | Accelerometer msg pointer |
Definition at line 276 of file marty_core.cpp.
| void MartyCore::battCB |
( |
const std_msgs::Float32::ConstPtr & |
msg | ) |
|
|
private |
| void MartyCore::celebSound |
( |
float |
sound_time | ) |
|
| void MartyCore::enableRobot |
( |
| ) |
|
| bool MartyCore::fallDisabled |
( |
| ) |
|
|
inline |
| float MartyCore::getBattery |
( |
| ) |
|
| void MartyCore::gpioCB |
( |
const marty_msgs::GPIOs::ConstPtr & |
msg | ) |
|
|
private |
| bool MartyCore::hasFallen |
( |
| ) |
|
|
inline |
| void MartyCore::jointCB |
( |
const marty_msgs::ServoMsg::ConstPtr & |
msg | ) |
|
|
private |
| int MartyCore::jointPosToServoCmd |
( |
int |
id, |
|
|
float |
pos |
|
) |
| |
| void MartyCore::jointsCB |
( |
const marty_msgs::ServoMsgArray::ConstPtr & |
msg | ) |
|
|
private |
| void MartyCore::kfCB |
( |
const std_msgs::String::ConstPtr & |
msg | ) |
|
|
inlineprivate |
| void MartyCore::loadKeyframes |
( |
std::string |
keyframes_name, |
|
|
int |
move_time = 0 |
|
) |
| |
| void MartyCore::loadParams |
( |
| ) |
|
|
protected |
| void MartyCore::loadSound |
( |
std::string |
sound_name | ) |
|
Loads sound file and if well-formed plays it.
Sound files are stored in the cfg/sounds folder in the ros_marty package as .csv files
- Parameters
-
| sound_name | Name of sound file without .csv |
Definition at line 427 of file marty_core.cpp.
| void MartyCore::playSound |
( |
float |
frequency, |
|
|
float |
duration, |
|
|
float |
freq2 = -1 |
|
) |
| |
| void MartyCore::playSoundArray |
( |
marty_msgs::SoundArray |
sound_array | ) |
|
| void MartyCore::readySound |
( |
| ) |
|
| void MartyCore::rosSetup |
( |
| ) |
|
|
protected |
| void MartyCore::sendKeyframes |
( |
marty_msgs::KeyframeArray |
k_array | ) |
|
|
private |
| bool MartyCore::setFallDetector |
( |
std_srvs::SetBool::Request & |
req, |
|
|
std_srvs::SetBool::Response & |
res |
|
) |
| |
|
private |
| bool MartyCore::setServo |
( |
int |
id, |
|
|
float |
angle |
|
) |
| |
| void MartyCore::setServoJointPos |
( |
std::string |
name, |
|
|
int |
pos |
|
) |
| |
| void MartyCore::setServoPos |
( |
int |
channel, |
|
|
int |
pos |
|
) |
| |
| void MartyCore::setServos |
( |
std::map< int, float > |
angles | ) |
|
| void MartyCore::setupJointStates |
( |
| ) |
|
|
private |
| void MartyCore::setupOdometry |
( |
| ) |
|
|
private |
| void MartyCore::soundCB |
( |
const std_msgs::String::ConstPtr & |
msg | ) |
|
|
inlineprivate |
| void MartyCore::stopRobot |
( |
| ) |
|
| void MartyCore::stopSound |
( |
| ) |
|
| void MartyCore::tfCB |
( |
const ros::TimerEvent & |
e | ) |
|
|
private |
Publishes TF and joint states on a timer callback.
Updates all Marty's TF, joint states and odometry information
- Parameters
-
| e | Timer event callback at 10Hz |
Definition at line 409 of file marty_core.cpp.
| void MartyCore::updateJointState |
( |
marty_msgs::ServoMsg |
servo | ) |
|
|
private |
Updates the joint state positions of every joint given servo cmds.
This function is called on every callback that updates a joint position, and publishes the appropriate updates to the joint_states topic.
- Parameters
-
| servo | The servo cmd message (Id and Position) |
Definition at line 215 of file marty_core.cpp.
| void MartyCore::updateOdom |
( |
| ) |
|
|
private |
This function updates the Odometry given Marty's motion and sensors.
Marty uses joint state data to estimate its pose as it moves. The change in position of the feet (Assuming the base_link of the robot moves inversely to the foot on the ground)
Definition at line 324 of file marty_core.cpp.
| marty_msgs::Accelerometer MartyCore::accel_ |
|
private |
| ros::Subscriber MartyCore::accel_sub_ |
|
private |
| ros::Subscriber MartyCore::batt_sub_ |
|
private |
| float MartyCore::battery_max_ |
|
private |
| float MartyCore::battery_min_ |
|
private |
| float MartyCore::battery_val_ |
|
private |
| bool MartyCore::calibrated_ |
|
private |
| geometry_msgs::TransformStamped MartyCore::cam_tf_ |
|
private |
| double MartyCore::camera_ori_ |
|
private |
| ros::Publisher MartyCore::enable_pub_ |
|
private |
| std_msgs::Bool MartyCore::enable_robot_ |
|
private |
| ros::ServiceServer MartyCore::fall_dis_srv_ |
|
private |
| bool MartyCore::fall_disable_ |
|
private |
| double MartyCore::fall_thr_ |
|
private |
| std_msgs::Bool MartyCore::falling_ |
|
private |
| ros::Publisher MartyCore::falling_pub_ |
|
private |
| ros::ServiceClient MartyCore::get_gpio_config_ |
|
private |
| ros::Subscriber MartyCore::gpio_sub_ |
|
private |
| marty_msgs::GPIOs MartyCore::gpios_val_ |
|
private |
| std::deque<float> MartyCore::jangles_ |
| ros::Subscriber MartyCore::joint_sub_ |
|
private |
| sensor_msgs::JointState MartyCore::joints_ |
|
private |
| ros::Publisher MartyCore::joints_pub_ |
|
private |
| ros::Subscriber MartyCore::joints_sub_ |
|
private |
| ros::Publisher MartyCore::keyframe_pub_ |
|
private |
| ros::Subscriber MartyCore::keyframe_sub_ |
|
private |
| geometry_msgs::TransformStamped MartyCore::l_f_tf_ |
|
private |
| ros::NodeHandle MartyCore::nh_ |
|
protected |
| bool MartyCore::odom_accel_ |
|
private |
| tf2_ros::TransformBroadcaster MartyCore::odom_br_ |
|
private |
| bool MartyCore::odom_setup_ |
|
private |
| geometry_msgs::TransformStamped MartyCore::odom_tf_ |
|
private |
| std::deque<double> MartyCore::pitch_ |
|
private |
| tf2::Quaternion MartyCore::quat_ori_ |
|
private |
| geometry_msgs::TransformStamped MartyCore::r_f_tf_ |
|
private |
| std::deque<double> MartyCore::roll_ |
|
private |
| ros::Publisher MartyCore::servo_array_pub_ |
|
private |
| marty_msgs::ServoMsg MartyCore::servo_msg_ |
|
private |
| ros::Publisher MartyCore::servo_pub_ |
|
private |
| ros::ServiceClient MartyCore::set_gpio_config_ |
|
private |
| bool MartyCore::simulated_ |
|
private |
| ros::Publisher MartyCore::sound_pub_ |
|
private |
| ros::Subscriber MartyCore::sound_sub_ |
|
private |
| tf2_ros::TransformBroadcaster MartyCore::tf_br_ |
|
private |
| tf2_ros::Buffer MartyCore::tf_buff_ |
|
private |
| tf2_ros::TransformListener MartyCore::tf_ls_ |
|
private |
| ros::Timer MartyCore::tf_timer_ |
|
private |
| std::deque<double> MartyCore::yaw_ |
|
private |
The documentation for this class was generated from the following files: