create_driver.cpp
Go to the documentation of this file.
1 
29 
30 #include <tf/transform_datatypes.h>
31 
32 #include <string>
33 
35  : nh_(nh),
36  priv_nh_("~"),
37  diagnostics_(),
38  model_(create::RobotModel::CREATE_2),
39  is_running_slowly_(false)
40 {
41  bool create_one;
42  std::string robot_model_name;
43  priv_nh_.param<std::string>("dev", dev_, "/dev/ttyUSB0");
44  priv_nh_.param<std::string>("robot_model", robot_model_name, "CREATE_2");
45  priv_nh_.param<std::string>("base_frame", base_frame_, "base_footprint");
46  priv_nh_.param<std::string>("odom_frame", odom_frame_, "odom");
47  priv_nh_.param<double>("latch_cmd_duration", latch_duration_, 0.2);
48  priv_nh_.param<double>("loop_hz", loop_hz_, 10.0);
49  priv_nh_.param<bool>("publish_tf", publish_tf_, true);
50 
51  if (robot_model_name == "ROOMBA_400")
52  {
54  }
55  else if (robot_model_name == "CREATE_1")
56  {
58  }
59  else if (robot_model_name == "CREATE_2")
60  {
62  }
63  else
64  {
65  ROS_FATAL_STREAM("[CREATE] Robot model \"" + robot_model_name + "\" is not known.");
66  ros::shutdown();
67  return;
68  }
69 
70  ROS_INFO_STREAM("[CREATE] \"" << robot_model_name << "\" selected");
71 
72  priv_nh_.param<int>("baud", baud_, model_.getBaud());
73 
75 
76  if (!robot_->connect(dev_, baud_))
77  {
78  ROS_FATAL("[CREATE] Failed to establish serial connection with Create.");
79  ros::shutdown();
80  }
81 
82  ROS_INFO("[CREATE] Connection established.");
83 
84  // Start in full control mode
85  robot_->setMode(create::MODE_FULL);
86 
87  // Show robot's battery level
88  ROS_INFO("[CREATE] Battery level %.2f %%", (robot_->getBatteryCharge() / robot_->getBatteryCapacity()) * 100.0);
89 
90  // Set frame_id's
91  mode_msg_.header.frame_id = base_frame_;
92  bumper_msg_.header.frame_id = base_frame_;
93  charging_state_msg_.header.frame_id = base_frame_;
94  tf_odom_.header.frame_id = odom_frame_;
95  tf_odom_.child_frame_id = base_frame_;
96  odom_msg_.header.frame_id = odom_frame_;
97  odom_msg_.child_frame_id = base_frame_;
98  joint_state_msg_.name.resize(2);
99  joint_state_msg_.position.resize(2);
100  joint_state_msg_.velocity.resize(2);
101  joint_state_msg_.effort.resize(2);
102  joint_state_msg_.name[0] = "left_wheel_joint";
103  joint_state_msg_.name[1] = "right_wheel_joint";
104 
105  // Populate intial covariances
106  for (int i = 0; i < 36; i++)
107  {
108  odom_msg_.pose.covariance[i] = COVARIANCE[i];
109  odom_msg_.twist.covariance[i] = COVARIANCE[i];
110  }
111 
112  // Setup subscribers
113  cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &CreateDriver::cmdVelCallback, this);
114  debris_led_sub_ = nh.subscribe("debris_led", 10, &CreateDriver::debrisLEDCallback, this);
115  spot_led_sub_ = nh.subscribe("spot_led", 10, &CreateDriver::spotLEDCallback, this);
116  dock_led_sub_ = nh.subscribe("dock_led", 10, &CreateDriver::dockLEDCallback, this);
117  check_led_sub_ = nh.subscribe("check_led", 10, &CreateDriver::checkLEDCallback, this);
118  power_led_sub_ = nh.subscribe("power_led", 10, &CreateDriver::powerLEDCallback, this);
119  set_ascii_sub_ = nh.subscribe("set_ascii", 10, &CreateDriver::setASCIICallback, this);
120  dock_sub_ = nh.subscribe("dock", 10, &CreateDriver::dockCallback, this);
121  undock_sub_ = nh.subscribe("undock", 10, &CreateDriver::undockCallback, this);
122  define_song_sub_ = nh.subscribe("define_song", 10, &CreateDriver::defineSongCallback, this);
123  play_song_sub_ = nh.subscribe("play_song", 10, &CreateDriver::playSongCallback, this);
124 
125  // Setup publishers
126  odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
127  clean_btn_pub_ = nh.advertise<std_msgs::Empty>("clean_button", 30);
128  day_btn_pub_ = nh.advertise<std_msgs::Empty>("day_button", 30);
129  hour_btn_pub_ = nh.advertise<std_msgs::Empty>("hour_button", 30);
130  min_btn_pub_ = nh.advertise<std_msgs::Empty>("minute_button", 30);
131  dock_btn_pub_ = nh.advertise<std_msgs::Empty>("dock_button", 30);
132  spot_btn_pub_ = nh.advertise<std_msgs::Empty>("spot_button", 30);
133  voltage_pub_ = nh.advertise<std_msgs::Float32>("battery/voltage", 30);
134  current_pub_ = nh.advertise<std_msgs::Float32>("battery/current", 30);
135  charge_pub_ = nh.advertise<std_msgs::Float32>("battery/charge", 30);
136  charge_ratio_pub_ = nh.advertise<std_msgs::Float32>("battery/charge_ratio", 30);
137  capacity_pub_ = nh.advertise<std_msgs::Float32>("battery/capacity", 30);
138  temperature_pub_ = nh.advertise<std_msgs::Int16>("battery/temperature", 30);
139  charging_state_pub_ = nh.advertise<ca_msgs::ChargingState>("battery/charging_state", 30);
140  omni_char_pub_ = nh.advertise<std_msgs::UInt16>("ir_omni", 30);
141  mode_pub_ = nh.advertise<ca_msgs::Mode>("mode", 30);
142  bumper_pub_ = nh.advertise<ca_msgs::Bumper>("bumper", 30);
143  wheeldrop_pub_ = nh.advertise<std_msgs::Empty>("wheeldrop", 30);
144  wheel_joint_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
145 
146  // Setup diagnostics
147  diagnostics_.add("Battery Status", this, &CreateDriver::updateBatteryDiagnostics);
148  diagnostics_.add("Safety Status", this, &CreateDriver::updateSafetyDiagnostics);
149  diagnostics_.add("Serial Status", this, &CreateDriver::updateSerialDiagnostics);
151  diagnostics_.add("Driver Status", this, &CreateDriver::updateDriverDiagnostics);
152 
153  diagnostics_.setHardwareID(robot_model_name);
154 
155  ROS_INFO("[CREATE] Ready.");
156 }
157 
159 {
160  ROS_INFO("[CREATE] Destruct sequence initiated.");
161  robot_->disconnect();
162  delete robot_;
163 }
164 
165 void CreateDriver::cmdVelCallback(const geometry_msgs::TwistConstPtr& msg)
166 {
167  robot_->drive(msg->linear.x, msg->angular.z);
169 }
170 
171 void CreateDriver::debrisLEDCallback(const std_msgs::BoolConstPtr& msg)
172 {
173  robot_->enableDebrisLED(msg->data);
174 }
175 
176 void CreateDriver::spotLEDCallback(const std_msgs::BoolConstPtr& msg)
177 {
178  robot_->enableSpotLED(msg->data);
179 }
180 
181 void CreateDriver::dockLEDCallback(const std_msgs::BoolConstPtr& msg)
182 {
183  robot_->enableDockLED(msg->data);
184 }
185 
186 void CreateDriver::checkLEDCallback(const std_msgs::BoolConstPtr& msg)
187 {
188  robot_->enableCheckRobotLED(msg->data);
189 }
190 
191 void CreateDriver::powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr& msg)
192 {
193  if (msg->data.size() < 1)
194  {
195  ROS_ERROR("[CREATE] No values provided to set power LED");
196  }
197  else
198  {
199  if (msg->data.size() < 2)
200  {
201  robot_->setPowerLED(msg->data[0]);
202  }
203  else
204  {
205  robot_->setPowerLED(msg->data[0], msg->data[1]);
206  }
207  }
208 }
209 
210 void CreateDriver::setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg)
211 {
212  bool result;
213  if (msg->data.size() < 1)
214  {
215  ROS_ERROR("[CREATE] No ASCII digits provided");
216  }
217  else if (msg->data.size() < 2)
218  {
219  result = robot_->setDigitsASCII(msg->data[0], ' ', ' ', ' ');
220  }
221  else if (msg->data.size() < 3)
222  {
223  result = robot_->setDigitsASCII(msg->data[0], msg->data[1], ' ', ' ');
224  }
225  else if (msg->data.size() < 4)
226  {
227  result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], ' ');
228  }
229  else
230  {
231  result = robot_->setDigitsASCII(msg->data[0], msg->data[1], msg->data[2], msg->data[3]);
232  }
233 
234  if (!result)
235  {
236  ROS_ERROR("[CREATE] ASCII character out of range [32, 126]");
237  }
238 }
239 
240 void CreateDriver::dockCallback(const std_msgs::EmptyConstPtr& msg)
241 {
243 
244  if (model_.getVersion() <= create::V_2)
245  usleep(1000000); // Create 1 requires a delay (1 sec)
246 
247  // Call docking behaviour
248  robot_->dock();
249 }
250 
251 void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg)
252 {
253  // Switch robot back to FULL mode
255 }
256 
257 void CreateDriver::defineSongCallback(const ca_msgs::DefineSongConstPtr& msg)
258 {
259  if (!robot_->defineSong(msg->song, msg->length, &(msg->notes.front()), &(msg->durations.front())))
260  {
261  ROS_ERROR_STREAM("[CREATE] Failed to define song " << msg->song << " of length " << msg->length);
262  }
263 }
264 
265 void CreateDriver::playSongCallback(const ca_msgs::PlaySongConstPtr& msg)
266 {
267  if (!robot_->playSong(msg->song))
268  {
269  ROS_ERROR_STREAM("[CREATE] Failed to play song " << msg->song);
270  }
271 }
272 
274 {
275  publishOdom();
279  publishOmniChar();
280  publishMode();
283 
284  // If last velocity command was sent longer than latch duration, stop robot
286  {
287  robot_->drive(0, 0);
288  }
289 
290  return true;
291 }
292 
294 {
295  const float charge = robot_->getBatteryCharge();
296  const float capacity = robot_->getBatteryCapacity();
297  const create::ChargingState charging_state = robot_->getChargingState();
298  const float charge_ratio = charge / capacity;
299 
300  if (charging_state == create::CHARGE_FAULT)
301  {
302  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Charging fault reported by base");
303  }
304  else if (charge_ratio == 0)
305  {
306  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery reports no charge");
307  }
308  else if (charge_ratio < 0.1)
309  {
310  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery reports less than 10% charge");
311  }
312  else
313  {
314  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery is OK");
315  }
316 
317  stat.add("Charge (Ah)", charge);
318  stat.add("Capacity (Ah)", capacity);
319  stat.add("Temperature (Celsius)", robot_->getTemperature());
320  stat.add("Current (A)", robot_->getCurrent());
321  stat.add("Voltage (V)", robot_->getVoltage());
322 
323  switch (charging_state)
324  {
325  case create::CHARGE_NONE:
326  stat.add("Charging state", "Not charging");
327  break;
329  stat.add("Charging state", "Reconditioning");
330  break;
331  case create::CHARGE_FULL:
332  stat.add("Charging state", "Full charge");
333  break;
335  stat.add("Charging state", "Trickle charging");
336  break;
338  stat.add("Charging state", "Waiting");
339  break;
341  stat.add("Charging state", "Fault");
342  break;
343  }
344 }
345 
347 {
348  const bool is_wheeldrop = robot_->isWheeldrop();
349  const bool is_cliff = robot_->isCliff();
350  if (is_wheeldrop)
351  {
352  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheeldrop detected");
353  }
354  else if (is_cliff)
355  {
356  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff detected");
357  }
358  else
359  {
360  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "No safety issues detected");
361  }
362 
363  stat.add("Wheeldrop", is_wheeldrop);
364  stat.add("Cliff", is_cliff);
365 }
366 
368 {
369  const bool is_connected = robot_->connected();
370  const uint64_t corrupt_packets = robot_->getNumCorruptPackets();
371  const uint64_t total_packets = robot_->getTotalPackets();
372 
373  if (!is_connected)
374  {
375  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Serial port to base not open");
376  }
377  else if (corrupt_packets)
378  {
379  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN,
380  "Corrupt packets detected. If the number of corrupt packets is increasing, data may be unreliable");
381  }
382  else
383  {
384  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Serial connection is good");
385  }
386 
387  stat.add("Corrupt packets", corrupt_packets);
388  stat.add("Total packets", total_packets);
389 }
390 
392 {
393  const create::CreateMode mode = robot_->getMode();
394  switch (mode)
395  {
397  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown mode of operation");
398  break;
399  case create::MODE_OFF:
400  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Mode is set to OFF");
401  break;
403  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to PASSIVE");
404  break;
405  case create::MODE_SAFE:
406  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to SAFE");
407  break;
408  case create::MODE_FULL:
409  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to FULL");
410  break;
411  }
412 }
413 
415 {
416  if (is_running_slowly_)
417  {
418  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Internal loop running slowly");
419  }
420  else
421  {
422  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Maintaining loop frequency");
423  }
424 }
425 
427 {
428  create::Pose pose = robot_->getPose();
429  create::Vel vel = robot_->getVel();
430 
431  // Populate position info
432  geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.yaw);
433  odom_msg_.header.stamp = ros::Time::now();
434  odom_msg_.pose.pose.position.x = pose.x;
435  odom_msg_.pose.pose.position.y = pose.y;
436  odom_msg_.pose.pose.orientation = quat;
437 
438  // Populate velocity info
439  odom_msg_.twist.twist.linear.x = vel.x;
440  odom_msg_.twist.twist.linear.y = vel.y;
441  odom_msg_.twist.twist.angular.z = vel.yaw;
442 
443  // Update covariances
444  odom_msg_.pose.covariance[0] = static_cast<double>(pose.covariance[0]);
445  odom_msg_.pose.covariance[1] = pose.covariance[1];
446  odom_msg_.pose.covariance[5] = pose.covariance[2];
447  odom_msg_.pose.covariance[6] = pose.covariance[3];
448  odom_msg_.pose.covariance[7] = pose.covariance[4];
449  odom_msg_.pose.covariance[11] = pose.covariance[5];
450  odom_msg_.pose.covariance[30] = pose.covariance[6];
451  odom_msg_.pose.covariance[31] = pose.covariance[7];
452  odom_msg_.pose.covariance[35] = pose.covariance[8];
453  odom_msg_.twist.covariance[0] = vel.covariance[0];
454  odom_msg_.twist.covariance[1] = vel.covariance[1];
455  odom_msg_.twist.covariance[5] = vel.covariance[2];
456  odom_msg_.twist.covariance[6] = vel.covariance[3];
457  odom_msg_.twist.covariance[7] = vel.covariance[4];
458  odom_msg_.twist.covariance[11] = vel.covariance[5];
459  odom_msg_.twist.covariance[30] = vel.covariance[6];
460  odom_msg_.twist.covariance[31] = vel.covariance[7];
461  odom_msg_.twist.covariance[35] = vel.covariance[8];
462 
463  if (publish_tf_)
464  {
465  tf_odom_.header.stamp = ros::Time::now();
466  tf_odom_.transform.translation.x = pose.x;
467  tf_odom_.transform.translation.y = pose.y;
468  tf_odom_.transform.rotation = quat;
470  }
471 
473 }
474 
476 {
477  // Publish joint states
478  float wheelRadius = model_.getWheelDiameter() / 2.0;
479 
480  joint_state_msg_.header.stamp = ros::Time::now();
481  joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / wheelRadius;
482  joint_state_msg_.position[1] = robot_->getRightWheelDistance() / wheelRadius;
483  joint_state_msg_.velocity[0] = robot_->getRequestedLeftWheelVel() / wheelRadius;
484  joint_state_msg_.velocity[1] = robot_->getRequestedRightWheelVel() / wheelRadius;
486 }
487 
489 {
490  float32_msg_.data = robot_->getVoltage();
492  float32_msg_.data = robot_->getCurrent();
498  int16_msg_.data = robot_->getTemperature();
502 
503  const create::ChargingState charging_state = robot_->getChargingState();
504  charging_state_msg_.header.stamp = ros::Time::now();
505  switch (charging_state)
506  {
507  case create::CHARGE_NONE:
508  charging_state_msg_.state = charging_state_msg_.CHARGE_NONE;
509  break;
511  charging_state_msg_.state = charging_state_msg_.CHARGE_RECONDITION;
512  break;
513 
514  case create::CHARGE_FULL:
515  charging_state_msg_.state = charging_state_msg_.CHARGE_FULL;
516  break;
517 
519  charging_state_msg_.state = charging_state_msg_.CHARGE_TRICKLE;
520  break;
521 
523  charging_state_msg_.state = charging_state_msg_.CHARGE_WAITING;
524  break;
525 
527  charging_state_msg_.state = charging_state_msg_.CHARGE_FAULT;
528  break;
529  }
531 }
532 
534 {
536  {
538  }
539  if (robot_->isDayButtonPressed())
540  {
542  }
544  {
546  }
547  if (robot_->isMinButtonPressed())
548  {
550  }
552  {
554  }
556  {
558  }
559 }
560 
562 {
563  uint8_t ir_char = robot_->getIROmni();
564  uint16_msg_.data = ir_char;
566  // TODO(jacobperron): Publish info based on character, such as dock in sight
567 }
568 
570 {
571  const create::CreateMode mode = robot_->getMode();
572  mode_msg_.header.stamp = ros::Time::now();
573  switch (mode)
574  {
575  case create::MODE_OFF:
576  mode_msg_.mode = mode_msg_.MODE_OFF;
577  break;
579  mode_msg_.mode = mode_msg_.MODE_PASSIVE;
580  break;
581  case create::MODE_SAFE:
582  mode_msg_.mode = mode_msg_.MODE_SAFE;
583  break;
584  case create::MODE_FULL:
585  mode_msg_.mode = mode_msg_.MODE_FULL;
586  break;
587  default:
588  ROS_ERROR("[CREATE] Unknown mode detected");
589  break;
590  }
592 }
593 
595 {
596  bumper_msg_.header.stamp = ros::Time::now();
597  bumper_msg_.is_left_pressed = robot_->isLeftBumper();
598  bumper_msg_.is_right_pressed = robot_->isRightBumper();
599 
600  if (model_.getVersion() >= create::V_3)
601  {
602  bumper_msg_.is_light_left = robot_->isLightBumperLeft();
603  bumper_msg_.is_light_front_left = robot_->isLightBumperFrontLeft();
604  bumper_msg_.is_light_center_left = robot_->isLightBumperCenterLeft();
605  bumper_msg_.is_light_right = robot_->isLightBumperRight();
606  bumper_msg_.is_light_front_right = robot_->isLightBumperFrontRight();
607  bumper_msg_.is_light_center_right = robot_->isLightBumperCenterRight();
608 
609  bumper_msg_.light_signal_left = robot_->getLightSignalLeft();
610  bumper_msg_.light_signal_front_left = robot_->getLightSignalFrontLeft();
611  bumper_msg_.light_signal_center_left = robot_->getLightSignalCenterLeft();
612  bumper_msg_.light_signal_right = robot_->getLightSignalRight();
613  bumper_msg_.light_signal_front_right = robot_->getLightSignalFrontRight();
614  bumper_msg_.light_signal_center_right = robot_->getLightSignalCenterRight();
615  }
616 
618 }
619 
621 {
622  if (robot_->isWheeldrop())
624 }
625 
627 {
628  update();
630  ros::spinOnce();
631 }
632 
634 {
635  ros::Rate rate(loop_hz_);
636  while (ros::ok())
637  {
638  spinOnce();
639 
640  is_running_slowly_ = !rate.sleep();
641  if (is_running_slowly_)
642  {
643  ROS_WARN("[CREATE] Loop running slowly.");
644  }
645  }
646 }
647 
648 int main(int argc, char** argv)
649 {
650  ros::init(argc, argv, "ca_driver");
651  ros::NodeHandle nh;
652 
653  CreateDriver create_driver(nh);
654 
655  try
656  {
657  create_driver.spin();
658  }
659  catch (std::runtime_error& ex)
660  {
661  ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what());
662  return 1;
663  }
664  return 0;
665 }
uint64_t getTotalPackets() const
void updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
virtual void spinOnce()
uint8_t getIROmni() const
std::string odom_frame_
Definition: create_driver.h:86
void debrisLEDCallback(const std_msgs::BoolConstPtr &msg)
ros::Subscriber define_song_sub_
ros::Subscriber power_led_sub_
#define ROS_FATAL(...)
ros::Subscriber set_ascii_sub_
ros::Time last_cmd_vel_time_
Definition: create_driver.h:75
void checkLEDCallback(const std_msgs::BoolConstPtr &msg)
void publishBumperInfo()
ros::Subscriber dock_led_sub_
ros::Subscriber spot_led_sub_
void publish(const boost::shared_ptr< M > &message) const
bool isWheeldrop() const
unsigned int getBaud() const
bool isLightBumperFrontLeft() const
ros::Publisher wheel_joint_pub_
bool setDigitsASCII(const uint8_t &digit1, const uint8_t &digit2, const uint8_t &digit3, const uint8_t &digit4) const
void updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
ros::Publisher voltage_pub_
void summary(unsigned char lvl, const std::string s)
ros::Publisher charging_state_pub_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
bool is_running_slowly_
Definition: create_driver.h:81
void playSongCallback(const ca_msgs::PlaySongConstPtr &msg)
static RobotModel CREATE_1
void publishJointState()
ros::Publisher omni_char_pub_
void setHardwareID(const std::string &hwid)
bool isLeftBumper() const
ros::Publisher min_btn_pub_
void defineSongCallback(const ca_msgs::DefineSongConstPtr &msg)
create::ChargingState getChargingState() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Publisher bumper_pub_
void add(const std::string &name, TaskFunction f)
ros::Subscriber check_led_sub_
bool dock() const
std_msgs::Float32 float32_msg_
Definition: create_driver.h:77
tf::TransformBroadcaster tf_broadcaster_
Definition: create_driver.h:68
int8_t getTemperature() const
diagnostic_updater::Updater diagnostics_
Definition: create_driver.h:69
CreateDriver(ros::NodeHandle &nh)
double loop_hz_
Definition: create_driver.h:88
bool isHourButtonPressed() const
uint16_t getLightSignalFrontLeft() const
std_msgs::UInt16 uint16_msg_
Definition: create_driver.h:78
ros::Publisher temperature_pub_
ros::Subscriber debris_led_sub_
bool isLightBumperCenterRight() const
float getRequestedLeftWheelVel() const
ca_msgs::ChargingState charging_state_msg_
Definition: create_driver.h:71
void publishWheeldrop()
#define ROS_WARN(...)
void dockCallback(const std_msgs::EmptyConstPtr &msg)
void publishButtonPresses() const
ros::Publisher clean_btn_pub_
ros::Publisher odom_pub_
float getRightWheelDistance() const
create::Pose getPose() const
ros::Subscriber play_song_sub_
float getRequestedRightWheelVel() const
ca_msgs::Bumper bumper_msg_
Definition: create_driver.h:72
bool enableDockLED(const bool &enable)
bool setPowerLED(const uint8_t &power, const uint8_t &intensity=255)
ros::Publisher day_btn_pub_
void cmdVelCallback(const geometry_msgs::TwistConstPtr &msg)
bool isLightBumperFrontRight() const
float getBatteryCharge() const
float getCurrent() const
ros::Publisher charge_ratio_pub_
nav_msgs::Odometry odom_msg_
Definition: create_driver.h:73
ProtocolVersion getVersion() const
void setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr &msg)
bool playSong(const uint8_t &songNumber) const
bool connected() const
MODE_UNAVAILABLE
void updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
double latch_duration_
Definition: create_driver.h:87
virtual void spin()
ros::Publisher dock_btn_pub_
std::vector< float > covariance
create::Vel getVel() const
#define ROS_FATAL_STREAM(args)
uint64_t getNumCorruptPackets() const
uint16_t getLightSignalCenterLeft() const
std_msgs::Int16 int16_msg_
Definition: create_driver.h:79
ros::Subscriber cmd_vel_sub_
#define ROS_INFO(...)
void spotLEDCallback(const std_msgs::BoolConstPtr &msg)
bool isLightBumperLeft() const
void publishOmniChar()
bool param(const std::string &param_name, T &param_val, const T &default_val) const
float getWheelDiameter() const
void updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
bool enableCheckRobotLED(const bool &enable)
ROSCPP_DECL bool ok()
bool enableSpotLED(const bool &enable)
void sendTransform(const StampedTransform &transform)
void powerLEDCallback(const std_msgs::UInt8MultiArrayConstPtr &msg)
void publishBatteryInfo()
bool isMinButtonPressed() const
bool isCleanButtonPressed() const
static const double COVARIANCE[36]
Definition: create_driver.h:56
bool drive(const float &xVel, const float &angularVel)
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher hour_btn_pub_
bool isDockButtonPressed() const
bool enableDebrisLED(const bool &enable)
ros::Subscriber dock_sub_
float getVoltage() const
uint16_t getLightSignalLeft() const
std::string base_frame_
Definition: create_driver.h:85
bool sleep()
bool isRightBumper() const
bool isDayButtonPressed() const
ros::Publisher capacity_pub_
ros::Publisher wheeldrop_pub_
std::string dev_
Definition: create_driver.h:84
ros::Publisher charge_pub_
uint16_t getLightSignalCenterRight() const
#define ROS_INFO_STREAM(args)
std_msgs::Empty empty_msg_
Definition: create_driver.h:76
bool isSpotButtonPressed() const
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
uint16_t getLightSignalRight() const
void undockCallback(const std_msgs::EmptyConstPtr &msg)
void dockLEDCallback(const std_msgs::BoolConstPtr &msg)
static Time now()
ros::NodeHandle priv_nh_
create::Create * robot_
Definition: create_driver.h:66
ROSCPP_DECL void shutdown()
float getBatteryCapacity() const
ros::Publisher mode_pub_
CHARGE_RECONDITION
static RobotModel CREATE_2
ros::Subscriber undock_sub_
void add(const std::string &key, const T &val)
bool isCliff() const
create::RobotModel model_
Definition: create_driver.h:67
#define ROS_ERROR_STREAM(args)
ros::Publisher current_pub_
ROSCPP_DECL void spinOnce()
ros::Publisher spot_btn_pub_
static RobotModel ROOMBA_400
#define ROS_ERROR(...)
geometry_msgs::TransformStamped tf_odom_
Definition: create_driver.h:74
float getLeftWheelDistance() const
bool setMode(const create::CreateMode &mode)
ca_msgs::Mode mode_msg_
Definition: create_driver.h:70
create::CreateMode getMode()
bool isLightBumperRight() const
sensor_msgs::JointState joint_state_msg_
Definition: create_driver.h:80
bool isLightBumperCenterLeft() const
uint16_t getLightSignalFrontRight() const
bool defineSong(const uint8_t &songNumber, const uint8_t &songLength, const uint8_t *notes, const float *durations) const


ca_driver
Author(s): Jacob Perron
autogenerated on Mon Jun 10 2019 13:03:52