fix #30: add method to check if encoder ticks are published

diffbot_base/include/diffbot_base/diffbot_hw_interface.h

@@ -90,6 +90,14 @@ namespace diffbot_base
  */
  virtual void write(const ros::Time& time, const ros::Duration& period);
 
+ /** \brief Check if encoder ticks are received.
+ * 
+ * This function blocks until the sub_encoder_ticks_ subscriber receives encoder ticks.
+ *
+ * \param timeout Minimum time to wait for receiving encoder ticks
+ */
+ bool isReceivingEncoderTicks(const ros::Duration &timeout=ros::Duration(1)) const;
+
  /** \brief Helper for debugging a joint's state */
  virtual void printState();
  std::string printStateHelper();
@@ -170,10 +178,11 @@ namespace diffbot_base
  ros::Publisher pub_left_motor_value_;
  ros::Publisher pub_right_motor_value_;
 
- // Declare subscribers for the wheel encoders
- ros::Subscriber sub_left_encoder_ticks_;
- ros::Subscriber sub_right_encoder_ticks_;
+ // Declare subscriber for the wheel encoders
+ // This subscriber receives the encoder ticks in the custom diffbot_msgs/Encoder message
+ ros::Subscriber sub_encoder_ticks_;
 
+ // Array to store the received encoder tick values from the \ref sub_encoder_ticks_ subscriber
  int encoder_ticks_[NUM_JOINTS];
 
  PID pids_[NUM_JOINTS];

diffbot_base/src/diffbot_hw_interface.cpp

@@ -43,10 +43,13 @@ namespace diffbot_base
  pub_right_motor_value_ = nh_.advertise<std_msgs::Int32>("motor_right", 10);
 
  // Setup subscriber for the wheel encoders
- sub_left_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this);
+ sub_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this);
 
  // Initialize the hardware interface
  init(nh_, nh_);
+
+ // Wait for encoder messages being published
+ isReceivingEncoderTicks(ros::Duration(10));
  }
 
 
@@ -101,6 +104,7 @@ namespace diffbot_base
  registerInterface(&velocity_joint_interface_);
 
  ROS_INFO("... Done Initializing DiffBot Hardware Interface");
+
  return true;
  }
 
@@ -208,6 +212,30 @@ namespace diffbot_base
  ROS_INFO_STREAM(std::endl << ss.str());
  }
 
+ bool DiffBotHWInterface::isReceivingEncoderTicks(const ros::Duration &timeout) const
+ {
+ ROS_INFO("Get number of encoder ticks publishers");
+
+ ros::Time start = ros::Time::now();
+ int num_publishers = sub_encoder_ticks_.getNumPublishers();
+ ROS_INFO("Waiting for encoder ticks being published...");
+ while ((num_publishers == 0) && (ros::Time::now() < start + timeout))
+ {
+ ros::Duration(0.1).sleep();
+ num_publishers = sub_encoder_ticks_.getNumPublishers();
+ }
+ if (num_publishers == 0)
+ {
+ ROS_ERROR("No encoder ticks publishers. Timeout reached.");
+ }
+ else
+ {
+ ROS_INFO_STREAM("Number of encoder ticks publishers: " << num_publishers);
+ }
+
+ return (num_publishers > 0);
+ }
+
  void DiffBotHWInterface::loadURDF(const ros::NodeHandle &nh, std::string param_name)
  {
  std::string urdf_string;