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;
}