15 ROS_INFO(
"MartyCore Ready!");
19 ros::param::del(
"/marty");
26 ERR(
"Marty has not been calibrated before!\n");
27 WARN(
"Please use 'roslaunch ros_marty calibration.launch' to calibrate\n");
28 ros::param::del(
"/marty");
34 std::string zero_p =
NAMES[id] +
"/zero";
35 std::string max_p =
NAMES[id] +
"/max";
36 std::string min_p =
NAMES[id] +
"/min";
37 std::string dir_p =
NAMES[id] +
"/dir";
38 std::string mult_p =
NAMES[id] +
"/mult";
39 ros::param::get(zero_p,
joint_[
id].cmdZero);
40 ros::param::get(max_p,
joint_[
id].cmdMax);
41 ros::param::get(min_p,
joint_[
id].cmdMin);
42 ros::param::get(dir_p,
joint_[
id].cmdDir);
43 ros::param::get(mult_p,
joint_[
id].cmdMult);
52 if (
simulated_) { ROS_WARN(
"Marty is being simulated"); }
54 marty_msgs::ServoMsg joint;
71 cam_tf_.header.frame_id =
"base_link";
72 cam_tf_.child_frame_id =
"camera";
73 cam_tf_.transform.translation.x = 0.023;
74 cam_tf_.transform.translation.y = 0.0;
75 cam_tf_.transform.translation.z = 0.027;
78 tf2::Quaternion q(-0.627, 0.627, -0.327, 0.327);
80 cam_tf_.transform.rotation = tf2::toMsg(q);
92 odom_tf_.child_frame_id =
"base_link";
93 odom_tf_.transform.translation.x = 0;
94 odom_tf_.transform.translation.y = 0;
95 odom_tf_.transform.translation.z = 0;
108 ROS_INFO(
"Waiting for rosserial to start...\n");
113 servo_pub_ =
nh_.advertise<marty_msgs::ServoMsg>(
"servo", 10);
115 joints_pub_ =
nh_.advertise<sensor_msgs::JointState>(
"joint_states", 10);
116 sound_pub_ =
nh_.advertise<marty_msgs::SoundArray>(
"sound", 10);
145 std_srvs::SetBool::Response& res) {
150 joints_.name.push_back(
"face_joint");
151 joints_.name.push_back(
"left_arm_servo_gear_joint");
152 joints_.name.push_back(
"left_arm_gear_joint");
153 joints_.name.push_back(
"right_arm_servo_gear_joint");
154 joints_.name.push_back(
"right_arm_gear_joint");
155 joints_.name.push_back(
"right_eye_joint");
156 joints_.name.push_back(
"left_eye_joint");
157 joints_.name.push_back(
"left_hip_link1_joint");
158 joints_.name.push_back(
"left_hip_link2_joint");
159 joints_.name.push_back(
"left_hip_link3_joint");
160 joints_.name.push_back(
"left_hip_servo_link_joint");
161 joints_.name.push_back(
"left_twist_servo_holder1_joint");
162 joints_.name.push_back(
"left_twist_shaft_joint");
163 joints_.name.push_back(
"left_knee_link1_joint");
164 joints_.name.push_back(
"left_knee_link2_joint");
165 joints_.name.push_back(
"left_knee_link3_joint");
166 joints_.name.push_back(
"left_knee_servo_link_joint");
167 joints_.name.push_back(
"left_foot_joint");
168 joints_.name.push_back(
"right_hip_link1_joint");
169 joints_.name.push_back(
"right_hip_link2_joint");
170 joints_.name.push_back(
"right_hip_link3_joint");
171 joints_.name.push_back(
"right_hip_servo_link_joint");
172 joints_.name.push_back(
"right_twist_servo_holder1_joint");
173 joints_.name.push_back(
"right_twist_shaft_joint");
174 joints_.name.push_back(
"right_knee_link1_joint");
175 joints_.name.push_back(
"right_knee_link2_joint");
176 joints_.name.push_back(
"right_knee_link3_joint");
177 joints_.name.push_back(
"right_knee_servo_link_joint");
178 joints_.name.push_back(
"right_foot_joint");
179 for (
int j = 0; j <
joints_.name.size(); ++j) {
180 joints_.position.push_back(0.0);
186 l_f_tf_ =
tf_buff_.lookupTransform(
"base_link",
"left_foot", ros::Time(0));
187 r_f_tf_ =
tf_buff_.lookupTransform(
"base_link",
"right_foot", ros::Time(0));
189 }
catch (tf2::TransformException& ex) { ROS_WARN(
"%s", ex.what()); }
197 for (
int id = 0;
id < msg->servo_msg.size(); ++id) {
216 if (servo.servo_id == 0) {
223 }
else if (servo.servo_id == 1) {
226 }
else if (servo.servo_id == 2) {
233 }
else if (servo.servo_id == 3) {
240 }
else if (servo.servo_id == 4) {
243 }
else if (servo.servo_id == 5) {
250 }
else if (servo.servo_id == 6) {
254 }
else if (servo.servo_id == 7) {
258 }
else if (servo.servo_id == 8) {
280 ROS_WARN_STREAM(
"Robot Falling! " <<
accel_.z << std::endl);
288 ROS_WARN_STREAM(
"Robot Stable! " <<
accel_.z << std::endl);
300 roll_.push_front(roll);
pitch_.push_front(pitch);
yaw_.push_front(yaw);
301 int s =
roll_.size();
302 for (
int i = 1; i < s; ++i) {
305 roll = roll / s; pitch = pitch / s; yaw = yaw / s;
307 r_ = roll;
p_ = -pitch;
328 geometry_msgs::TransformStamped l_f_tf, r_f_tf;
329 geometry_msgs::Transform tf_diff;
333 tf_diff.translation.x = tf_diff.translation.y = tf_diff.translation.z = 0;
334 tf_diff.rotation.x = tf_diff.rotation.y = tf_diff.rotation.z = 0;
335 tf_diff.rotation.w = 1;
336 l_f_tf.transform = r_f_tf.transform = tf_diff;
339 l_f_tf =
tf_buff_.lookupTransform(
"base_link",
"left_foot", ros::Time(0));
340 r_f_tf =
tf_buff_.lookupTransform(
"base_link",
"right_foot", ros::Time(0));
341 }
catch (tf2::TransformException& ex) { ROS_WARN(
"%s", ex.what()); }
344 double rql_, pql_, yql_, rql, pql, yql, rqr_, pqr_, yqr_, rqr, pqr, yqr;
345 tf2::Quaternion ql_(
l_f_tf_.transform.rotation.x,
l_f_tf_.transform.rotation.y,
347 ql_.normalize(); tf2::Matrix3x3(ql_).getRPY(rql_, pql_, yql_);
348 tf2::Quaternion ql(l_f_tf.transform.rotation.x, l_f_tf.transform.rotation.y,
349 l_f_tf.transform.rotation.z, l_f_tf.transform.rotation.w);
350 ql.normalize(); tf2::Matrix3x3(ql).getRPY(rql, pql, yql);
351 tf2::Quaternion qr_(
r_f_tf_.transform.rotation.x,
r_f_tf_.transform.rotation.y,
353 qr_.normalize(); tf2::Matrix3x3(qr_).getRPY(rqr_, pqr_, yqr_);
354 tf2::Quaternion qr(r_f_tf.transform.rotation.x, r_f_tf.transform.rotation.y,
355 r_f_tf.transform.rotation.z, r_f_tf.transform.rotation.w);
356 qr.normalize(); tf2::Matrix3x3(qr).getRPY(rqr, pqr, yqr);
359 if (l_f_tf.transform.translation.z < r_f_tf.transform.translation.z) {
361 tf_diff.translation.x =
362 l_f_tf_.transform.translation.x - l_f_tf.transform.translation.x;
363 tf_diff.translation.y =
364 l_f_tf_.transform.translation.y - l_f_tf.transform.translation.y;
365 tf_diff.translation.z =
366 l_f_tf_.transform.translation.z - l_f_tf.transform.translation.z;
367 }
else if (l_f_tf.transform.translation.z > r_f_tf.transform.translation.z) {
369 tf_diff.translation.x =
370 r_f_tf_.transform.translation.x - r_f_tf.transform.translation.x;
371 tf_diff.translation.y =
372 r_f_tf_.transform.translation.y - r_f_tf.transform.translation.y;
373 tf_diff.translation.z =
374 r_f_tf_.transform.translation.z - r_f_tf.transform.translation.z;
376 dy = ((yql_ - yql) + (yqr_ - yqr)) / 2;
377 tf_diff.translation.x =
378 ((
l_f_tf_.transform.translation.x - l_f_tf.transform.translation.x) +
379 (
r_f_tf_.transform.translation.x - r_f_tf.transform.translation.x)) / 2;
380 tf_diff.translation.y =
381 ((
l_f_tf_.transform.translation.y - l_f_tf.transform.translation.y) +
382 (
r_f_tf_.transform.translation.y - r_f_tf.transform.translation.y)) / 2;
383 tf_diff.translation.z =
384 ((
l_f_tf_.transform.translation.z - l_f_tf.transform.translation.z) +
385 (
r_f_tf_.transform.translation.z - r_f_tf.transform.translation.z)) / 2;
390 (tf_diff.translation.x * cos(-
y_)) + (tf_diff.translation.y * sin(-
y_));
392 (tf_diff.translation.y * cos(-
y_)) + (tf_diff.translation.x * -sin(-
y_));
393 odom_tf_.transform.translation.z += tf_diff.translation.z;
428 std::ifstream soundFile;
429 std::string path = ros::package::getPath(
"ros_marty");
430 soundFile.open(path +
"/cfg/sounds/" + sound_name +
".csv");
431 if (soundFile.is_open()) {
432 marty_msgs::SoundArray sound_array;
433 marty_msgs::Sound sound;
434 std::string line;
char c =
',';
435 while (std::getline(soundFile, line)) {
436 std::istringstream iss(line);
438 if (!(iss >> f1 >> c >> f2 >> c >> t)) { ROS_ERROR(
"FILE ERROR");
break;}
439 sound.freq1 = f1; sound.freq2 = f2; sound.duration = t;
440 sound_array.sound.push_back(sound);
445 ROS_ERROR_STREAM(
"COULD NOT OPEN FILE: " << sound_name <<
".csv");
454 float short_s = sound_time / 28;
455 float long_s = short_s * 4;
456 float vlong_s = short_s * 12;
457 marty_msgs::SoundArray sound_array;
458 marty_msgs::Sound sound;
459 sound.freq1 =
NOTE_G5; sound.freq2 =
NOTE_G5; sound.duration = long_s;
460 sound_array.sound.push_back(sound);
461 sound.freq1 =
SILENCE; sound.freq2 =
SILENCE; sound.duration = short_s;
462 sound_array.sound.push_back(sound);
463 sound.freq1 =
NOTE_G5; sound.freq2 =
NOTE_G5; sound.duration = short_s;
464 sound_array.sound.push_back(sound);
465 sound.freq1 =
SILENCE; sound.freq2 =
SILENCE; sound.duration = short_s;
466 sound_array.sound.push_back(sound);
467 sound.freq1 =
NOTE_G5; sound.freq2 =
NOTE_G5; sound.duration = short_s;
468 sound_array.sound.push_back(sound);
469 sound.freq1 =
SILENCE; sound.freq2 =
SILENCE; sound.duration = short_s;
470 sound_array.sound.push_back(sound);
471 sound.freq1 =
NOTE_C6; sound.freq2 =
NOTE_C6; sound.duration = long_s;
472 sound_array.sound.push_back(sound);
473 sound.freq1 =
SILENCE; sound.freq2 =
SILENCE; sound.duration = short_s;
474 sound_array.sound.push_back(sound);
475 sound.freq1 =
NOTE_G5; sound.freq2 =
NOTE_G5; sound.duration = short_s;
476 sound_array.sound.push_back(sound);
477 sound.freq1 =
SILENCE; sound.freq2 =
SILENCE; sound.duration = short_s;
478 sound_array.sound.push_back(sound);
479 sound.freq1 =
NOTE_C6; sound.freq2 =
NOTE_C6; sound.duration = vlong_s;
480 sound_array.sound.push_back(sound);
485 if (freq2 == -1) {freq2 = frequency;}
486 marty_msgs::Sound sound;
487 sound.freq1 = frequency; sound.freq2 = freq2; sound.duration = duration;
488 marty_msgs::SoundArray sound_array;
489 sound_array.sound.push_back(sound);
494 for (
int s = 0; s < (sound_array.sound.size() / 8); ++s) {
495 marty_msgs::SoundArray sounds;
496 for (
int i = (0 + (s * 8)); i < (8 + (s * 8)); ++i) {
497 sounds.sound.push_back(sound_array.sound[i]);
501 marty_msgs::SoundArray sounds;
502 for (
int i = 0; i < (sound_array.sound.size() % 8); ++i) {
503 sounds.sound.push_back(
504 sound_array.sound[i + (sound_array.sound.size() / 8) * 8]);
510 marty_msgs::Sound sound;
511 sound.freq1 = 0; sound.freq2 = sound.freq1; sound.duration = 0;
512 marty_msgs::SoundArray sound_array;
513 sound_array.sound.push_back(sound);
518 std::ifstream keyframeFile;
519 std::string path = ros::package::getPath(
"ros_marty");
520 keyframeFile.open(path +
"/cfg/keyframes/" + keyframes_name +
".csv");
521 if (keyframeFile.is_open()) {
522 float total_time = 0; std::string line;
char c =
',';
523 while (std::getline(keyframeFile, line)) {
524 std::istringstream iss(line);
526 if (!(iss >> t)) {ROS_ERROR(
"NO T DATA");
break;}
530 float new_time = (float)move_time / 1000.0;
531 if (move_time == 0) { new_time = total_time; }
533 keyframeFile.clear();
534 keyframeFile.seekg(0, std::ios::beg);
535 marty_msgs::KeyframeArray k_a;
536 while (std::getline(keyframeFile, line)) {
537 std::istringstream iss(line);
538 marty_msgs::Keyframe k;
539 if (!(iss >> k.time)) {ROS_ERROR(
"NO KT DATA");
break;}
540 k.time = k.time * (new_time / total_time);
541 marty_msgs::ServoMsgArray s_a;
542 marty_msgs::ServoMsg s;
544 while (iss >> c >>
id >> c >> cmd) {
545 s.servo_id = id; s.servo_cmd = cmd;
547 s_a.servo_msg.push_back(s);
550 k_a.keyframe.push_back(k);
553 keyframeFile.close();
555 ROS_WARN_STREAM(
"COULD NOT OPEN FILE: " << keyframes_name <<
".csv");
564 float cmd = float(
joint_[
id].cmdZero) +
566 cmd = fmin(cmd,
joint_[
id].cmdMax); cmd = fmax(cmd,
joint_[
id].cmdMin);
572 catch (
const std::exception& e) {std::cerr << e.what();};
595 if (id < 0 || id >=
NUMJOINTS) {
return false; }
605 marty_msgs::ServoMsgArray servo_msg_array;
606 for (std::map<int, float>::iterator i = angles.begin(); i != angles.end();
608 marty_msgs::ServoMsg servo_msg;
609 servo_msg.servo_id = i->first;
611 servo_msg_array.servo_msg.push_back(servo_msg);
612 jangles_[i->first] = angles.at(i->first);
tf2_ros::TransformBroadcaster odom_br_
void setServos(std::map< int, float > angles)
void jointsCB(const marty_msgs::ServoMsgArray::ConstPtr &msg)
ros::Subscriber keyframe_sub_
void setServoJointPos(std::string name, int pos)
void kfCB(const std_msgs::String::ConstPtr &msg)
std::deque< float > jangles_
int jointPosToServoCmd(int id, float pos)
ros::Subscriber sound_sub_
geometry_msgs::TransformStamped cam_tf_
void battCB(const std_msgs::Float32::ConstPtr &msg)
ros::Publisher joints_pub_
ros::ServiceServer fall_dis_srv_
ros::Publisher falling_pub_
ros::Subscriber gpio_sub_
bool setFallDetector(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
std::deque< double > yaw_
std::deque< double > roll_
Marty Core header providing access to Marty methods.
void gpioCB(const marty_msgs::GPIOs::ConstPtr &msg)
geometry_msgs::TransformStamped r_f_tf_
const std::vector< std::string > NAMES
ros::Subscriber batt_sub_
MartyCore(ros::NodeHandle &nh)
void updateJointState(marty_msgs::ServoMsg servo)
Updates the joint state positions of every joint given servo cmds.
void sendKeyframes(marty_msgs::KeyframeArray k_array)
void loadSound(std::string sound_name)
Loads sound file and if well-formed plays it.
geometry_msgs::TransformStamped odom_tf_
ros::Publisher enable_pub_
void tfCB(const ros::TimerEvent &e)
Publishes TF and joint states on a timer callback.
void soundCB(const std_msgs::String::ConstPtr &msg)
geometry_msgs::TransformStamped l_f_tf_
bool setServo(int id, float angle)
ros::Subscriber joint_sub_
marty_msgs::GPIOs gpios_val_
ros::Publisher servo_array_pub_
marty_msgs::ServoMsg servo_msg_
void playSound(float frequency, float duration, float freq2=-1)
void updateOdom()
This function updates the Odometry given Marty's motion and sensors.
void jointCB(const marty_msgs::ServoMsg::ConstPtr &msg)
ros::Publisher servo_pub_
ros::Publisher sound_pub_
ros::Publisher keyframe_pub_
const std::map< std::string, int > JOINT_NAMES
void loadKeyframes(std::string keyframes_name, int move_time=0)
void accelCB(const marty_msgs::Accelerometer::ConstPtr &msg)
Accelerometer data callback. Accelerometer data used to detect falls and for walking odometry...
ros::Subscriber joints_sub_
void setServoPos(int channel, int pos)
void playSoundArray(marty_msgs::SoundArray sound_array)
sensor_msgs::JointState joints_
tf2_ros::TransformBroadcaster tf_br_
ros::Subscriber accel_sub_
tf2::Quaternion quat_ori_
robotJoint joint_[NUMJOINTS]
marty_msgs::Accelerometer accel_
std::deque< double > pitch_
void celebSound(float sound_time)
std_msgs::Bool enable_robot_