Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
MartyCore Class Reference

#include <marty_core.hpp>

Public Member Functions

void celebSound (float sound_time)
 
void enableRobot ()
 
bool fallDisabled ()
 
float getBattery ()
 
bool hasFallen ()
 
int jointPosToServoCmd (int id, float pos)
 
void loadKeyframes (std::string keyframes_name, int move_time=0)
 
void loadSound (std::string sound_name)
 Loads sound file and if well-formed plays it. More...
 
 MartyCore (ros::NodeHandle &nh)
 
void playSound (float frequency, float duration, float freq2=-1)
 
void playSoundArray (marty_msgs::SoundArray sound_array)
 
void readySound ()
 
bool setServo (int id, float angle)
 
void setServoJointPos (std::string name, int pos)
 
void setServoPos (int channel, int pos)
 
void setServos (std::map< int, float > angles)
 
void stopRobot ()
 
void stopSound ()
 
 ~MartyCore ()
 

Public Attributes

std::deque< float > jangles_
 
robotJoint joint_ [NUMJOINTS]
 

Protected Member Functions

void init ()
 
void loadParams ()
 
void rosSetup ()
 

Protected Attributes

ros::NodeHandle nh_
 

Private Member Functions

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...
 

Private Attributes

marty_msgs::Accelerometer accel_
 
ros::Subscriber accel_sub_
 
ros::Subscriber batt_sub_
 
float battery_max_
 
float battery_min_
 
float battery_val_
 
bool calibrated_
 
geometry_msgs::TransformStamped cam_tf_
 
bool camera_
 
double camera_ori_
 
ros::Publisher enable_pub_
 
std_msgs::Bool enable_robot_
 
ros::ServiceServer fall_dis_srv_
 
bool fall_disable_
 
double fall_thr_
 
std_msgs::Bool falling_
 
ros::Publisher falling_pub_
 
ros::ServiceClient get_gpio_config_
 
ros::Subscriber gpio_sub_
 
marty_msgs::GPIOs gpios_val_
 
ros::Subscriber joint_sub_
 
sensor_msgs::JointState joints_
 
ros::Publisher joints_pub_
 
ros::Subscriber joints_sub_
 
ros::Publisher keyframe_pub_
 
ros::Subscriber keyframe_sub_
 
geometry_msgs::TransformStamped l_f_tf_
 
bool odom_accel_
 
tf2_ros::TransformBroadcaster odom_br_
 
bool odom_setup_
 
geometry_msgs::TransformStamped odom_tf_
 
double p_
 
std::deque< double > pitch_
 
tf2::Quaternion quat_ori_
 
double r_
 
geometry_msgs::TransformStamped r_f_tf_
 
std::deque< double > roll_
 
ros::Publisher servo_array_pub_
 
marty_msgs::ServoMsg servo_msg_
 
ros::Publisher servo_pub_
 
ros::ServiceClient set_gpio_config_
 
bool simulated_
 
ros::Publisher sound_pub_
 
ros::Subscriber sound_sub_
 
tf2_ros::TransformBroadcaster tf_br_
 
tf2_ros::Buffer tf_buff_
 
tf2_ros::TransformListener tf_ls_
 
ros::Timer tf_timer_
 
double y_
 
std::deque< double > yaw_
 

Detailed Description

Definition at line 57 of file marty_core.hpp.

Constructor & Destructor Documentation

MartyCore::MartyCore ( ros::NodeHandle &  nh)

Definition at line 11 of file marty_core.cpp.

MartyCore::~MartyCore ( )

Definition at line 18 of file marty_core.cpp.

Member Function Documentation

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
msgAccelerometer msg pointer

Definition at line 276 of file marty_core.cpp.

void MartyCore::battCB ( const std_msgs::Float32::ConstPtr &  msg)
private

Definition at line 310 of file marty_core.cpp.

void MartyCore::celebSound ( float  sound_time)

Definition at line 453 of file marty_core.cpp.

void MartyCore::enableRobot ( )

Definition at line 584 of file marty_core.cpp.

bool MartyCore::fallDisabled ( )
inline

Definition at line 89 of file marty_core.hpp.

float MartyCore::getBattery ( )

Definition at line 202 of file marty_core.cpp.

void MartyCore::gpioCB ( const marty_msgs::GPIOs::ConstPtr &  msg)
private

Definition at line 314 of file marty_core.cpp.

bool MartyCore::hasFallen ( )
inline

Definition at line 88 of file marty_core.hpp.

void MartyCore::init ( )
protected

Definition at line 62 of file marty_core.cpp.

void MartyCore::jointCB ( const marty_msgs::ServoMsg::ConstPtr &  msg)
private

Definition at line 192 of file marty_core.cpp.

int MartyCore::jointPosToServoCmd ( int  id,
float  pos 
)

Definition at line 563 of file marty_core.cpp.

void MartyCore::jointsCB ( const marty_msgs::ServoMsgArray::ConstPtr &  msg)
private

Definition at line 196 of file marty_core.cpp.

void MartyCore::kfCB ( const std_msgs::String::ConstPtr &  msg)
inlineprivate

Definition at line 107 of file marty_core.hpp.

void MartyCore::loadKeyframes ( std::string  keyframes_name,
int  move_time = 0 
)

Definition at line 517 of file marty_core.cpp.

void MartyCore::loadParams ( )
protected

Definition at line 23 of file marty_core.cpp.

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_nameName of sound file without .csv

Definition at line 427 of file marty_core.cpp.

void MartyCore::playSound ( float  frequency,
float  duration,
float  freq2 = -1 
)

Definition at line 484 of file marty_core.cpp.

void MartyCore::playSoundArray ( marty_msgs::SoundArray  sound_array)

Definition at line 493 of file marty_core.cpp.

void MartyCore::readySound ( )

Definition at line 449 of file marty_core.cpp.

void MartyCore::rosSetup ( )
protected

Definition at line 103 of file marty_core.cpp.

void MartyCore::sendKeyframes ( marty_msgs::KeyframeArray  k_array)
private

Definition at line 559 of file marty_core.cpp.

bool MartyCore::setFallDetector ( std_srvs::SetBool::Request &  req,
std_srvs::SetBool::Response &  res 
)
private

Definition at line 144 of file marty_core.cpp.

bool MartyCore::setServo ( int  id,
float  angle 
)

Definition at line 594 of file marty_core.cpp.

void MartyCore::setServoJointPos ( std::string  name,
int  pos 
)

Definition at line 570 of file marty_core.cpp.

void MartyCore::setServoPos ( int  channel,
int  pos 
)

Definition at line 578 of file marty_core.cpp.

void MartyCore::setServos ( std::map< int, float >  angles)

Definition at line 604 of file marty_core.cpp.

void MartyCore::setupJointStates ( )
private

Definition at line 149 of file marty_core.cpp.

void MartyCore::setupOdometry ( )
private

Definition at line 184 of file marty_core.cpp.

void MartyCore::soundCB ( const std_msgs::String::ConstPtr &  msg)
inlineprivate

Definition at line 104 of file marty_core.hpp.

void MartyCore::stopRobot ( )

Definition at line 589 of file marty_core.cpp.

void MartyCore::stopSound ( )

Definition at line 509 of file marty_core.cpp.

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
eTimer 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
servoThe 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.

Member Data Documentation

marty_msgs::Accelerometer MartyCore::accel_
private

Definition at line 130 of file marty_core.hpp.

ros::Subscriber MartyCore::accel_sub_
private

Definition at line 155 of file marty_core.hpp.

ros::Subscriber MartyCore::batt_sub_
private

Definition at line 156 of file marty_core.hpp.

float MartyCore::battery_max_
private

Definition at line 133 of file marty_core.hpp.

float MartyCore::battery_min_
private

Definition at line 134 of file marty_core.hpp.

float MartyCore::battery_val_
private

Definition at line 132 of file marty_core.hpp.

bool MartyCore::calibrated_
private

Definition at line 117 of file marty_core.hpp.

geometry_msgs::TransformStamped MartyCore::cam_tf_
private

Definition at line 173 of file marty_core.hpp.

bool MartyCore::camera_
private

Definition at line 126 of file marty_core.hpp.

double MartyCore::camera_ori_
private

Definition at line 127 of file marty_core.hpp.

ros::Publisher MartyCore::enable_pub_
private

Definition at line 146 of file marty_core.hpp.

std_msgs::Bool MartyCore::enable_robot_
private

Definition at line 141 of file marty_core.hpp.

ros::ServiceServer MartyCore::fall_dis_srv_
private

Definition at line 161 of file marty_core.hpp.

bool MartyCore::fall_disable_
private

Definition at line 121 of file marty_core.hpp.

double MartyCore::fall_thr_
private

Definition at line 122 of file marty_core.hpp.

std_msgs::Bool MartyCore::falling_
private

Definition at line 131 of file marty_core.hpp.

ros::Publisher MartyCore::falling_pub_
private

Definition at line 147 of file marty_core.hpp.

ros::ServiceClient MartyCore::get_gpio_config_
private

Definition at line 163 of file marty_core.hpp.

ros::Subscriber MartyCore::gpio_sub_
private

Definition at line 157 of file marty_core.hpp.

marty_msgs::GPIOs MartyCore::gpios_val_
private

Definition at line 135 of file marty_core.hpp.

std::deque<float> MartyCore::jangles_

Definition at line 94 of file marty_core.hpp.

robotJoint MartyCore::joint_[NUMJOINTS]

Definition at line 93 of file marty_core.hpp.

ros::Subscriber MartyCore::joint_sub_
private

Definition at line 153 of file marty_core.hpp.

sensor_msgs::JointState MartyCore::joints_
private

Definition at line 143 of file marty_core.hpp.

ros::Publisher MartyCore::joints_pub_
private

Definition at line 150 of file marty_core.hpp.

ros::Subscriber MartyCore::joints_sub_
private

Definition at line 154 of file marty_core.hpp.

ros::Publisher MartyCore::keyframe_pub_
private

Definition at line 152 of file marty_core.hpp.

ros::Subscriber MartyCore::keyframe_sub_
private

Definition at line 159 of file marty_core.hpp.

geometry_msgs::TransformStamped MartyCore::l_f_tf_
private

Definition at line 175 of file marty_core.hpp.

ros::NodeHandle MartyCore::nh_
protected

Definition at line 59 of file marty_core.hpp.

bool MartyCore::odom_accel_
private

Definition at line 123 of file marty_core.hpp.

tf2_ros::TransformBroadcaster MartyCore::odom_br_
private

Definition at line 169 of file marty_core.hpp.

bool MartyCore::odom_setup_
private

Definition at line 118 of file marty_core.hpp.

geometry_msgs::TransformStamped MartyCore::odom_tf_
private

Definition at line 174 of file marty_core.hpp.

double MartyCore::p_
private

Definition at line 166 of file marty_core.hpp.

std::deque<double> MartyCore::pitch_
private

Definition at line 137 of file marty_core.hpp.

tf2::Quaternion MartyCore::quat_ori_
private

Definition at line 167 of file marty_core.hpp.

double MartyCore::r_
private

Definition at line 166 of file marty_core.hpp.

geometry_msgs::TransformStamped MartyCore::r_f_tf_
private

Definition at line 176 of file marty_core.hpp.

std::deque<double> MartyCore::roll_
private

Definition at line 136 of file marty_core.hpp.

ros::Publisher MartyCore::servo_array_pub_
private

Definition at line 149 of file marty_core.hpp.

marty_msgs::ServoMsg MartyCore::servo_msg_
private

Definition at line 142 of file marty_core.hpp.

ros::Publisher MartyCore::servo_pub_
private

Definition at line 148 of file marty_core.hpp.

ros::ServiceClient MartyCore::set_gpio_config_
private

Definition at line 162 of file marty_core.hpp.

bool MartyCore::simulated_
private

Definition at line 124 of file marty_core.hpp.

ros::Publisher MartyCore::sound_pub_
private

Definition at line 151 of file marty_core.hpp.

ros::Subscriber MartyCore::sound_sub_
private

Definition at line 158 of file marty_core.hpp.

tf2_ros::TransformBroadcaster MartyCore::tf_br_
private

Definition at line 168 of file marty_core.hpp.

tf2_ros::Buffer MartyCore::tf_buff_
private

Definition at line 170 of file marty_core.hpp.

tf2_ros::TransformListener MartyCore::tf_ls_
private

Definition at line 171 of file marty_core.hpp.

ros::Timer MartyCore::tf_timer_
private

Definition at line 172 of file marty_core.hpp.

double MartyCore::y_
private

Definition at line 166 of file marty_core.hpp.

std::deque<double> MartyCore::yaw_
private

Definition at line 138 of file marty_core.hpp.


The documentation for this class was generated from the following files:


ros_marty
Author(s): Alejandro Bordallo
autogenerated on Fri Jul 21 2017 14:51:45