From 645028298d192c26e4051664033cd5471203f17d Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 10 Jul 2019 09:55:48 +0200
Subject: [PATCH] Added an exception catch in the constructor.
---
src/darwin_robot.cpp | 40 +++++++++++++++++++++++-----------------
1 file changed, 23 insertions(+), 17 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index ca6c693..600d083 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -1,5 +1,6 @@
#include "darwin_robot.h"
#include "darwin_robot_exceptions.h"
+#include "dynamixelexceptions.h"
#include <iostream>
#include <sys/types.h>
#include <sys/stat.h>
@@ -82,23 +83,28 @@ CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_s
if(num_buses>0)
{
sleep(1);
- this->robot_device=dyn_server->get_device(id,dyn_version2);
- this->robot_id=id;
- /* wait for the firmware to finish the scan */
- do{
- /* get the current manager status */
- this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
- usleep(10000);
- }while(this->manager_status&MANAGER_SCANNING);
- /* get the number of servos detected */
- this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
- /* get the current action status */
- this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
- /* get the current smart charger status (detected or not)*/
- this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
- this->min_limit_current=MIN_LIMIT_CURRENT;
- this->max_limit_current=MAX_LIMIT_CURRENT;
-
+ try{
+ this->robot_device=dyn_server->get_device(id,dyn_version2);
+ this->robot_id=id;
+ /* wait for the firmware to finish the scan */
+ do{
+ /* get the current manager status */
+ this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&this->manager_status);
+ usleep(10000);
+ }while(this->manager_status&MANAGER_SCANNING);
+ /* get the number of servos detected */
+ this->robot_device->read_byte_register(DARWIN_MM_NUM_SERVOS,&this->manager_num_servos);
+ /* get the current action status */
+ this->robot_device->read_byte_register(DARWIN_ACTION_CNTRL,&this->action_status);
+ /* get the current smart charger status (detected or not)*/
+ this->robot_device->read_byte_register(DARWIN_SMART_CHARGER_CNTRL,&this->smart_charger_status);
+ this->min_limit_current=MIN_LIMIT_CURRENT;
+ this->max_limit_current=MAX_LIMIT_CURRENT;
+ }catch(CDynamixelServerException &e){
+ this->dyn_server->free_device(id);
+ std::cout << "exception" << std::endl;
+ throw e;
+ }
}
else
{
--
GitLab