int main()

in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [1375:1459]


int main(int argc, char **argv) {
  ros::init(argc, argv, "locobot_controller");
  ros::NodeHandle node_handle("");

  std::string port_name = "/dev/ttyUSB0";
  uint32_t baud_rate = 57600;

  if (argc < 2) {
    ROS_ERROR("Please set '-port_name' and  '-baud_rate' arguments for connected Dynamixels");
    return 0;
  } else {
    port_name = argv[1];
    baud_rate = atoi(argv[2]);
  }

  LoCoBotController dynamixel_controller;

  bool result = false;

  std::string yaml_file = node_handle.param<std::string>("dynamixel_info", "");

  result = dynamixel_controller.initWorkbench(port_name, baud_rate);
  if (!result) {
    ROS_ERROR("Please check USB port name");
    return 0;
  }

  result = dynamixel_controller.getDynamixelsInfo(yaml_file);
  if (!result) {
    ROS_ERROR("Please check YAML file");
    return 0;
  }

  result = dynamixel_controller.loadDynamixels();
  if (!result) {
    ROS_ERROR("Please check Dynamixel ID or BaudRate");
    return 0;
  }

  result = dynamixel_controller.initDynamixels();
  if (!result) {
    ROS_ERROR("Please check control table (http://emanual.robotis.com/#control-table)");
    return 0;
  }

  result = dynamixel_controller.initControlItems();
  if (!result) {
    ROS_ERROR("Please check control items");
    return 0;
  }

  result = dynamixel_controller.initSDKHandlers();
  if (!result) {
    ROS_ERROR("Failed to set Dynamixel SDK Handler");
    return 0;
  }

  dynamixel_controller.initPublisher();
  dynamixel_controller.initSubscriber();
  dynamixel_controller.initServer();
  dynamixel_controller.initActionlib();

  ros::Timer read_timer = node_handle.createTimer(ros::Duration(dynamixel_controller.getReadPeriod()),
                                                  &LoCoBotController::readCallback, &dynamixel_controller);
  ros::Timer publish_timer = node_handle.createTimer(ros::Duration(dynamixel_controller.getPublishPeriod()),
                                                     &LoCoBotController::publishCallback, &dynamixel_controller);
  ros::Timer gripper_timer = node_handle.createTimer(ros::Duration(dynamixel_controller.getPublishPeriod()),
                                                     &LoCoBotController::gripperController, &dynamixel_controller);
  ros::Timer hardware_timer2 = node_handle.createTimer(ros::Duration(dynamixel_controller.getPublishPeriod()),
                                                      &LoCoBotController::hardwareStatusPublish, &dynamixel_controller);
  bool status;

  ros::Rate loop_rate(50);

  while (ros::ok()) {
    status = dynamixel_controller.controlLoop();
    if (not status) {
      break;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}