marty_core.cpp
Go to the documentation of this file.
1 
10 
11 MartyCore::MartyCore(ros::NodeHandle& nh) : nh_(nh), tf_ls_(tf_buff_) {
12  this->loadParams();
13  this->init();
14  this->rosSetup();
15  ROS_INFO("MartyCore Ready!");
16 }
17 
19  ros::param::del("/marty");
20  nh_.deleteParam("~");
21 }
22 
24  nh_.param("calibrated", calibrated_, false);
25  if (!calibrated_) {
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");
29  nh_.deleteParam("~");
30  ros::shutdown();
31  exit(0);
32  }
33  for (int id = 0; id < NUMJOINTS; ++id) {
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);
44  }
45 
46  nh_.param("check_fall", fall_disable_, true);
47  nh_.param("fall_threshold", fall_thr_, 0.9);
48  nh_.param("odom_accel", odom_accel_, true);
49  nh_.param("camera", camera_, false);
50  if (camera_) {nh_.param("camera_ori", camera_ori_, 15.0);}
51  nh_.param("simulated", simulated_, false);
52  if (simulated_) { ROS_WARN("Marty is being simulated"); }
53 
54  marty_msgs::ServoMsg joint;
55  for (int id = 0; id < NUMJOINTS; ++id) {
56  joint.servo_id = id;
57  joint.servo_cmd = joint_[id].cmdZero;
58  // servo_msg_array_.servo_msg.push_back(joint);
59  }
60 }
61 
63  falling_.data = false;
64  battery_max_ = 8.4;
65  battery_min_ = 6.8;
66  odom_setup_ = false;
67  for (int ji = 0; ji < NUMJOINTS; ji++) { jangles_.push_back(0); }
68 
69  // Camera TF
70  if (camera_) {
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;
76  // tf2::Quaternion q(-0.54168, 0.54168, -0.45452, 0.45452);
77  // tf2::Quaternion q(-0.596, 0.596, -0.380, 0.380); // 25 deg
78  tf2::Quaternion q(-0.627, 0.627, -0.327, 0.327); // 35 deg
79  q.normalize();
80  cam_tf_.transform.rotation = tf2::toMsg(q);
81  // double ori = ((90 - camera_ori_) / 180) * M_PI;
82  // ROS_INFO_STREAM("CamOri: " << camera_ori_ << " ORI: " << ori <<
83  // " PI: " << M_PI);
84  // q.setRPY(-M_PI, ori, (M_PI / 2)); // TODO: This is borked
85  // ROS_INFO_STREAM("X: " << q.x() << " Y: " << q.y() <<
86  // " Z: " << q.z() << " W: " << q.w());
87  // cam_tf_.transform.rotation.x = tf2::toMsg(q);
88  }
89 
90  // Odom TF
91  odom_tf_.header.frame_id = "odom";
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;
96  odom_tf_.transform.rotation.x = 0;
97  odom_tf_.transform.rotation.y = 0;
98  odom_tf_.transform.rotation.z = 0;
99  odom_tf_.transform.rotation.w = 1;
100  this->setupJointStates();
101 }
102 
104  // PUBLISHERS
105  enable_pub_ = nh_.advertise<std_msgs::Bool>("enable_motors", 10);
106  if (!simulated_) {
107  while (enable_pub_.getNumSubscribers() == 0) {
108  ROS_INFO("Waiting for rosserial to start...\n");
109  sleepms(500);
110  }
111  }
112  falling_pub_ = nh_.advertise<std_msgs::Bool>("falling", 1, true);
113  servo_pub_ = nh_.advertise<marty_msgs::ServoMsg>("servo", 10);
114  servo_array_pub_ = nh_.advertise<marty_msgs::ServoMsgArray>("servo_array", 10);
115  joints_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 10);
116  sound_pub_ = nh_.advertise<marty_msgs::SoundArray>("sound", 10);
117  keyframe_pub_ = nh_.advertise<marty_msgs::KeyframeArray>("keyframes", 10);
118  // SUBSCRIBERS
119  joint_sub_ = nh_.subscribe("servo", 1000, &MartyCore::jointCB, this);
120  joints_sub_ = nh_.subscribe("servo_array", 1000, &MartyCore::jointsCB, this);
121  accel_sub_ = nh_.subscribe("accel", 1000, &MartyCore::accelCB, this);
122  batt_sub_ = nh_.subscribe("battery", 1000, &MartyCore::battCB, this);
123  gpio_sub_ = nh_.subscribe("gpios", 1000, &MartyCore::gpioCB, this);
124  sound_sub_ = nh_.subscribe("sound_file", 1000, &MartyCore::soundCB, this);
125  keyframe_sub_ = nh_.subscribe("keyframe_file", 1000, &MartyCore::kfCB, this);
126  // SERVICES
127  fall_dis_srv_ = nh_.advertiseService("fall_disable",
129  // ros::service::waitForService("set_gpio_config");
130  // set_gpio_config_ =
131  // nh_.serviceClient<marty_msgs::GPIOConfig>("set_gpio_config");
132  // ros::service::waitForService("get_gpio_config");
133  // get_gpio_config_ =
134  // nh_.serviceClient<marty_msgs::GPIOConfig>("get_gpio_config");
135  // marty_msgs::GPIOConfig srv;
136  // srv.request.config[0] = marty_msgs::GPIOConfig::Request::DIGITAL_IN;
137  // if (set_gpio_config_.call(srv)) {
138  // if (srv.response.success) {ROS_INFO("SUCCESS!");} else {ROS_WARN("NAY!");}
139  // } else { ROS_ERROR("Failed to call set gpio service!"); }
140  // TIMERS
141  tf_timer_ = nh_.createTimer(ros::Duration(0.1), &MartyCore::tfCB, this);
142 }
143 
144 bool MartyCore::setFallDetector(std_srvs::SetBool::Request& req,
145  std_srvs::SetBool::Response& res) {
146  fall_disable_ = req.data; res.success = true; return true;
147 }
148 
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);
181  }
182 }
183 
185  try { // Wait until transformation data from feet is received
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));
188  odom_setup_ = true;
189  } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); }
190 }
191 
192 void MartyCore::jointCB(const marty_msgs::ServoMsg::ConstPtr& msg) {
193  this->updateJointState(*msg);
194 }
195 
196 void MartyCore::jointsCB(const marty_msgs::ServoMsgArray::ConstPtr& msg) {
197  for (int id = 0; id < msg->servo_msg.size(); ++id) {
198  this->updateJointState(msg->servo_msg[id]);
199  }
200 }
201 
203  float percentage = (battery_val_ - battery_min_) /
205  return percentage;
206 }
207 
215 void MartyCore::updateJointState(marty_msgs::ServoMsg servo) {
216  if (servo.servo_id == 0) {
217  joints_.position[10] =
218  ( (float)servo.servo_cmd - joint_[0].cmdZero ) / 123.3; // LHIP
219  joints_.position[7] = joints_.position[10]; // LHIP1
220  joints_.position[8] = joints_.position[10]; // LHIP2
221  joints_.position[9] = joints_.position[10]; // LHIP3
222  joints_.position[11] = -joints_.position[10]; // LKNEEBox
223  } else if (servo.servo_id == 1) {
224  joints_.position[12] =
225  -( (float)servo.servo_cmd - joint_[1].cmdZero ) / 123.3; // LTWIST
226  } else if (servo.servo_id == 2) {
227  joints_.position[16] =
228  -( (float)servo.servo_cmd - joint_[2].cmdZero ) / 123.3; // LKNEE
229  joints_.position[13] = joints_.position[16]; // LKNEE1
230  joints_.position[14] = joints_.position[16]; // LKNEE2
231  joints_.position[15] = joints_.position[16]; // LKNEE3
232  joints_.position[17] = -joints_.position[16]; // LFOOT
233  } else if (servo.servo_id == 3) {
234  joints_.position[21] =
235  ( (float)servo.servo_cmd - joint_[3].cmdZero ) / 123.3; // RHIP
236  joints_.position[18] = joints_.position[21]; // RHIP1
237  joints_.position[19] = joints_.position[21]; // RHIP2
238  joints_.position[20] = joints_.position[21]; // RHIP3
239  joints_.position[22] = -joints_.position[21]; // RKNEEBox
240  } else if (servo.servo_id == 4) {
241  joints_.position[23] =
242  -( (float)servo.servo_cmd - joint_[4].cmdZero ) / 123.3; // RTWIST
243  } else if (servo.servo_id == 5) {
244  joints_.position[27] =
245  -( (float)servo.servo_cmd - joint_[5].cmdZero ) / 123.3; // RKNEE
246  joints_.position[24] = joints_.position[27]; // RKNEE1
247  joints_.position[25] = joints_.position[27]; // RKNEE2
248  joints_.position[26] = joints_.position[27]; // RKNEE3
249  joints_.position[28] = -joints_.position[27]; // RFOOT
250  } else if (servo.servo_id == 6) {
251  joints_.position[1] =
252  ( (float)servo.servo_cmd - joint_[6].cmdZero ) / 123.3; // LARM-Gear
253  joints_.position[2] = -joints_.position[1] * 1.1875; // LARM
254  } else if (servo.servo_id == 7) {
255  joints_.position[3] =
256  ( (float)servo.servo_cmd - joint_[7].cmdZero ) / 123.3; // RARM-Gear
257  joints_.position[4] = -joints_.position[3] * 1.1875; // RARM
258  } else if (servo.servo_id == 8) {
259  joints_.position[6] =
260  ( (float)servo.servo_cmd - joint_[8].cmdZero ) / 123.3; // EYE-Left
261  joints_.position[5] = -joints_.position[6]; // EYE-Right
262  }
263 }
264 
276 void MartyCore::accelCB(const marty_msgs::Accelerometer::ConstPtr& msg) {
277  accel_ = *msg;
278  // Check if Robot is falling
279  if ((accel_.z < fall_thr_) && (falling_.data == false)) {
280  ROS_WARN_STREAM("Robot Falling! " << accel_.z << std::endl);
281  falling_.data = true;
282  if (fall_disable_) {
283  enable_robot_.data = false; enable_pub_.publish(enable_robot_);
284  }
285  falling_pub_.publish(falling_);
286  }
287  if ((accel_.z > fall_thr_) && (falling_.data == true)) {
288  ROS_WARN_STREAM("Robot Stable! " << accel_.z << std::endl);
289  falling_.data = false;
290  if (fall_disable_) {
291  enable_robot_.data = true; enable_pub_.publish(enable_robot_);
292  }
293  falling_pub_.publish(falling_);
294  }
295  // Obtain Roll, Pitch and Yaw angles from accelerometer data
296  double roll = atan2(accel_.z, accel_.y) - 1.57;
297  double pitch = atan2(accel_.z, accel_.x) - 1.57;
298  double yaw = atan2(accel_.y, accel_.x);
299  // Perform moving window average to filter accelerator noise
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) {
303  roll += roll_[i]; pitch += pitch_[i]; yaw += yaw_[i];
304  }
305  roll = roll / s; pitch = pitch / s; yaw = yaw / s;
306  roll_.resize(3); pitch_.resize(3); yaw_.resize(3);
307  r_ = roll; p_ = -pitch; // Yaw discarded due to gimbal lock
308 }
309 
310 void MartyCore::battCB(const std_msgs::Float32::ConstPtr& msg) {
311  battery_val_ = msg->data;
312 }
313 
314 void MartyCore::gpioCB(const marty_msgs::GPIOs::ConstPtr& msg) {
315  gpios_val_ = *msg;
316 }
317 
325  if (!odom_setup_) {
326  this->setupOdometry();
327  } else {
328  geometry_msgs::TransformStamped l_f_tf, r_f_tf; // Left/Right Foot TFs
329  geometry_msgs::Transform tf_diff; // TF change between time frames
330  double dy; // Yaw change from previous timestep
331 
332  // Initialise transformations
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;
337 
338  try { // Acquire latest feet transformation data
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()); }
342 
343  // Calculate feet previous and current roll, pitch, yaw orientations
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,
346  l_f_tf_.transform.rotation.z, l_f_tf_.transform.rotation.w);
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,
352  r_f_tf_.transform.rotation.z, r_f_tf_.transform.rotation.w);
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);
357 
358  // Compute change in Translation and Yaw given foot on ground
359  if (l_f_tf.transform.translation.z < r_f_tf.transform.translation.z) {
360  dy = yql_ - yql; // Left Foot Down
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) {
368  dy = yqr_ - yqr; // Right Foot Down
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;
375  } else {
376  dy = ((yql_ - yql) + (yqr_ - yqr)) / 2; // Both Feet Down, take average
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;
386  }
387  l_f_tf_ = l_f_tf; r_f_tf_ = r_f_tf; // Update feet tfs for next step
388  y_ += dy; // Update Yaw and Translation Odometry estimates
389  odom_tf_.transform.translation.x +=
390  (tf_diff.translation.x * cos(-y_)) + (tf_diff.translation.y * sin(-y_));
391  odom_tf_.transform.translation.y +=
392  (tf_diff.translation.y * cos(-y_)) + (tf_diff.translation.x * -sin(-y_));
393  odom_tf_.transform.translation.z += tf_diff.translation.z;
394  if (odom_accel_) { // Combine Accelerometer data
395  quat_ori_.setRPY(r_, p_, y_); quat_ori_.normalize();
396  } else { // Use only estimated Yaw
397  quat_ori_.setRPY(0, 0, y_); quat_ori_.normalize();
398  }
399  odom_tf_.transform.rotation = tf2::toMsg(quat_ori_);
400  }
401 }
402 
409 void MartyCore::tfCB(const ros::TimerEvent& e) {
410  // Publish Joint States
411  joints_.header.stamp = ros::Time::now(); joints_pub_.publish(joints_);
412  if (camera_) { // Camera transformation
413  cam_tf_.header.stamp = ros::Time::now(); tf_br_.sendTransform(cam_tf_);
414  }
415  // Odometry transformation
416  this->updateOdom();
417  odom_tf_.header.stamp = ros::Time::now(); odom_br_.sendTransform(odom_tf_);
418 }
419 
427 void MartyCore::loadSound(std::string sound_name) {
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 = ','; // Comma delimmited
435  while (std::getline(soundFile, line)) {
436  std::istringstream iss(line);
437  float f1, f2, t;
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);
441  }
442  this->playSoundArray(sound_array);
443  soundFile.close();
444  } else {
445  ROS_ERROR_STREAM("COULD NOT OPEN FILE: " << sound_name << ".csv");
446  }
447 }
448 
450  this->loadSound("ready");
451 }
452 
453 void MartyCore::celebSound(float sound_time) {
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);
481  this->playSoundArray(sound_array);
482 }
483 
484 void MartyCore::playSound(float frequency, float duration, float freq2) {
485  if (freq2 == -1) {freq2 = frequency;} // Disable chirp
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);
490  sound_pub_.publish(sound_array);
491 }
492 
493 void MartyCore::playSoundArray(marty_msgs::SoundArray sound_array) {
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]);
498  }
499  sound_pub_.publish(sounds);
500  }
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]);
505  }
506  sound_pub_.publish(sounds);
507 }
508 
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);
514  sound_pub_.publish(sound_array);
515 }
516 
517 void MartyCore::loadKeyframes(std::string keyframes_name, int move_time) {
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 = ','; // Comma delimmited
523  while (std::getline(keyframeFile, line)) {
524  std::istringstream iss(line);
525  float t;
526  if (!(iss >> t)) {ROS_ERROR("NO T DATA"); break;}
527  // ROS_WARN_STREAM("TIME: " << t);
528  total_time = t;
529  }
530  float new_time = (float)move_time / 1000.0;
531  if (move_time == 0) { new_time = total_time; } // Use Keyframe duration
532  // ROS_WARN_STREAM("NEW_TIME: " << new_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;} // Get Keyframe time
540  k.time = k.time * (new_time / total_time);
541  marty_msgs::ServoMsgArray s_a;
542  marty_msgs::ServoMsg s;
543  int id, cmd;
544  while (iss >> c >> id >> c >> cmd) {
545  s.servo_id = id; s.servo_cmd = cmd;
546  // ROS_WARN_STREAM("ID: " << id << " CMD: " << cmd);
547  s_a.servo_msg.push_back(s);
548  }
549  k.servo_array = s_a;
550  k_a.keyframe.push_back(k);
551  }
552  this->sendKeyframes(k_a);
553  keyframeFile.close();
554  } else {
555  ROS_WARN_STREAM("COULD NOT OPEN FILE: " << keyframes_name << ".csv");
556  }
557 }
558 
559 void MartyCore::sendKeyframes(marty_msgs::KeyframeArray k_array) {
560  keyframe_pub_.publish(k_array);
561 }
562 
563 int MartyCore::jointPosToServoCmd(int id, float pos) {
564  float cmd = float(joint_[id].cmdZero) +
565  (pos * joint_[id].cmdMult * float(joint_[id].cmdDir));
566  cmd = fmin(cmd, joint_[id].cmdMax); cmd = fmax(cmd, joint_[id].cmdMin);
567  return int(cmd);
568 }
569 
570 void MartyCore::setServoJointPos(std::string name, int pos) {
571  try {servo_msg_.servo_id = JOINT_NAMES.at(name);}
572  catch (const std::exception& e) {std::cerr << e.what();};
573  servo_msg_.servo_cmd = pos;
574  servo_pub_.publish(servo_msg_);
575  ros::spinOnce();
576 }
577 
578 void MartyCore::setServoPos(int channel, int pos) {
579  servo_msg_.servo_id = channel;
580  servo_msg_.servo_cmd = pos;
581  servo_pub_.publish(servo_msg_);
582 }
583 
585  enable_robot_.data = true;
586  enable_pub_.publish(enable_robot_);
587 }
588 
590  enable_robot_.data = false;
591  enable_pub_.publish(enable_robot_);
592 }
593 
594 bool MartyCore::setServo(int id, float angle) {
595  if (id < 0 || id >= NUMJOINTS) { return false; }
596  servo_msg_.servo_id = id;
597  servo_msg_.servo_cmd = jointPosToServoCmd(id, angle);
598  servo_pub_.publish(servo_msg_);
599  ros::spinOnce();
600  jangles_[id] = angle;
601  return true;
602 }
603 
604 void MartyCore::setServos(std::map<int, float> angles) {
605  marty_msgs::ServoMsgArray servo_msg_array;
606  for (std::map<int, float>::iterator i = angles.begin(); i != angles.end();
607  ++i) {
608  marty_msgs::ServoMsg servo_msg;
609  servo_msg.servo_id = i->first;
610  servo_msg.servo_cmd = jointPosToServoCmd(i->first, i->second);
611  servo_msg_array.servo_msg.push_back(servo_msg);
612  jangles_[i->first] = angles.at(i->first);
613  }
614  servo_array_pub_.publish(servo_msg_array);
615  ros::spinOnce();
616 }
tf2_ros::TransformBroadcaster odom_br_
Definition: marty_core.hpp:169
void setServos(std::map< int, float > angles)
Definition: marty_core.cpp:604
void jointsCB(const marty_msgs::ServoMsgArray::ConstPtr &msg)
Definition: marty_core.cpp:196
ros::Subscriber keyframe_sub_
Definition: marty_core.hpp:159
void stopSound()
Definition: marty_core.cpp:509
void setServoJointPos(std::string name, int pos)
Definition: marty_core.cpp:570
float getBattery()
Definition: marty_core.cpp:202
void kfCB(const std_msgs::String::ConstPtr &msg)
Definition: marty_core.hpp:107
std::deque< float > jangles_
Definition: marty_core.hpp:94
bool camera_
Definition: marty_core.hpp:126
int jointPosToServoCmd(int id, float pos)
Definition: marty_core.cpp:563
ros::Subscriber sound_sub_
Definition: marty_core.hpp:158
geometry_msgs::TransformStamped cam_tf_
Definition: marty_core.hpp:173
void battCB(const std_msgs::Float32::ConstPtr &msg)
Definition: marty_core.cpp:310
ros::Publisher joints_pub_
Definition: marty_core.hpp:150
ros::ServiceServer fall_dis_srv_
Definition: marty_core.hpp:161
bool fall_disable_
Definition: marty_core.hpp:121
ros::Publisher falling_pub_
Definition: marty_core.hpp:147
ros::Subscriber gpio_sub_
Definition: marty_core.hpp:157
bool setFallDetector(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
Definition: marty_core.cpp:144
#define WARN(x)
Definition: definitions.hpp:22
std::deque< double > yaw_
Definition: marty_core.hpp:138
std::deque< double > roll_
Definition: marty_core.hpp:136
double p_
Definition: marty_core.hpp:166
Marty Core header providing access to Marty methods.
#define NOTE_G5
bool simulated_
Definition: marty_core.hpp:124
void gpioCB(const marty_msgs::GPIOs::ConstPtr &msg)
Definition: marty_core.cpp:314
void readySound()
Definition: marty_core.cpp:449
float battery_min_
Definition: marty_core.hpp:134
geometry_msgs::TransformStamped r_f_tf_
Definition: marty_core.hpp:176
const std::vector< std::string > NAMES
Definition: definitions.hpp:44
void loadParams()
Definition: marty_core.cpp:23
ros::Subscriber batt_sub_
Definition: marty_core.hpp:156
MartyCore(ros::NodeHandle &nh)
Definition: marty_core.cpp:11
void updateJointState(marty_msgs::ServoMsg servo)
Updates the joint state positions of every joint given servo cmds.
Definition: marty_core.cpp:215
void sendKeyframes(marty_msgs::KeyframeArray k_array)
Definition: marty_core.cpp:559
ros::NodeHandle nh_
Definition: marty_core.hpp:59
void stopRobot()
Definition: marty_core.cpp:589
void init()
Definition: marty_core.cpp:62
std_msgs::Bool falling_
Definition: marty_core.hpp:131
void loadSound(std::string sound_name)
Loads sound file and if well-formed plays it.
Definition: marty_core.cpp:427
geometry_msgs::TransformStamped odom_tf_
Definition: marty_core.hpp:174
ros::Publisher enable_pub_
Definition: marty_core.hpp:146
void rosSetup()
Definition: marty_core.cpp:103
void tfCB(const ros::TimerEvent &e)
Publishes TF and joint states on a timer callback.
Definition: marty_core.cpp:409
float battery_val_
Definition: marty_core.hpp:132
bool odom_setup_
Definition: marty_core.hpp:118
void enableRobot()
Definition: marty_core.cpp:584
double camera_ori_
Definition: marty_core.hpp:127
void soundCB(const std_msgs::String::ConstPtr &msg)
Definition: marty_core.hpp:104
geometry_msgs::TransformStamped l_f_tf_
Definition: marty_core.hpp:175
bool setServo(int id, float angle)
Definition: marty_core.cpp:594
ros::Subscriber joint_sub_
Definition: marty_core.hpp:153
#define SILENCE
marty_msgs::GPIOs gpios_val_
Definition: marty_core.hpp:135
void setupOdometry()
Definition: marty_core.cpp:184
ros::Publisher servo_array_pub_
Definition: marty_core.hpp:149
void sleepms(int ms)
Definition: definitions.hpp:27
marty_msgs::ServoMsg servo_msg_
Definition: marty_core.hpp:142
float cmdMult
Definition: marty_core.hpp:53
void playSound(float frequency, float duration, float freq2=-1)
Definition: marty_core.cpp:484
#define ERR(x)
Definition: definitions.hpp:21
void updateOdom()
This function updates the Odometry given Marty&#39;s motion and sensors.
Definition: marty_core.cpp:324
ros::Timer tf_timer_
Definition: marty_core.hpp:172
void jointCB(const marty_msgs::ServoMsg::ConstPtr &msg)
Definition: marty_core.cpp:192
void setupJointStates()
Definition: marty_core.cpp:149
ros::Publisher servo_pub_
Definition: marty_core.hpp:148
ros::Publisher sound_pub_
Definition: marty_core.hpp:151
bool calibrated_
Definition: marty_core.hpp:117
ros::Publisher keyframe_pub_
Definition: marty_core.hpp:152
const std::map< std::string, int > JOINT_NAMES
Definition: definitions.hpp:86
void loadKeyframes(std::string keyframes_name, int move_time=0)
Definition: marty_core.cpp:517
double y_
Definition: marty_core.hpp:166
void accelCB(const marty_msgs::Accelerometer::ConstPtr &msg)
Accelerometer data callback. Accelerometer data used to detect falls and for walking odometry...
Definition: marty_core.cpp:276
ros::Subscriber joints_sub_
Definition: marty_core.hpp:154
void setServoPos(int channel, int pos)
Definition: marty_core.cpp:578
void playSoundArray(marty_msgs::SoundArray sound_array)
Definition: marty_core.cpp:493
tf2_ros::Buffer tf_buff_
Definition: marty_core.hpp:170
sensor_msgs::JointState joints_
Definition: marty_core.hpp:143
double fall_thr_
Definition: marty_core.hpp:122
#define NOTE_C6
tf2_ros::TransformBroadcaster tf_br_
Definition: marty_core.hpp:168
ros::Subscriber accel_sub_
Definition: marty_core.hpp:155
tf2::Quaternion quat_ori_
Definition: marty_core.hpp:167
float battery_max_
Definition: marty_core.hpp:133
robotJoint joint_[NUMJOINTS]
Definition: marty_core.hpp:93
marty_msgs::Accelerometer accel_
Definition: marty_core.hpp:130
std::deque< double > pitch_
Definition: marty_core.hpp:137
void celebSound(float sound_time)
Definition: marty_core.cpp:453
bool odom_accel_
Definition: marty_core.hpp:123
std_msgs::Bool enable_robot_
Definition: marty_core.hpp:141
double r_
Definition: marty_core.hpp:166


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