diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp index d342c28a8..4ea89f1f4 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp @@ -20,6 +20,7 @@ void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexI nh_->advertise(servo_state_pub_); nh_->advertise(servo_torque_state_pub_); nh_->advertiseService(servo_config_srv_); + nh_->advertiseService(board_info_srv_); //temp servo_state_msg_.servos_length = MAX_SERVO_NUM; @@ -27,12 +28,15 @@ void DirectServo::init(UART_HandleTypeDef* huart, ros::NodeHandle* nh, osMutexI servo_torque_state_msg_.torque_enable_length = MAX_SERVO_NUM; servo_torque_state_msg_.torque_enable = new uint8_t[MAX_SERVO_NUM]; - - servo_handler_.init(huart, mutex); servo_last_pub_time_ = 0; servo_torque_last_pub_time_ = 0; + + board_info_res_.boards_length = 1; + board_info_res_.boards = new spinal::BoardInfo[1]; + board_info_res_.boards[0].servos_length = servo_handler_.getServoNum(); + board_info_res_.boards[0].servos = new spinal::ServoInfo[servo_handler_.getServoNum()]; } void DirectServo::update() @@ -62,6 +66,17 @@ void DirectServo::sendData() servo_state_pub_.publish(&servo_state_msg_); servo_last_pub_time_ = now_time; } + if( now_time - servo_torque_last_pub_time_ >= SERVO_TORQUE_PUB_INTERVAL) + { + for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) { + const ServoData& s = servo_handler_.getServo()[i]; + if (s.send_data_flag_ != 0) { + servo_torque_state_msg_.torque_enable[i] = s.torque_enable_; + } + } + servo_torque_state_pub_.publish(&servo_torque_state_msg_); + servo_torque_last_pub_time_= now_time; + } } void DirectServo::torqueEnable(const std::map& servo_map) @@ -277,6 +292,29 @@ void DirectServo::servoConfigCallback(const spinal::SetDirectServoConfig::Reques // res.success = true; } +void DirectServo::boardInfoCallback(const spinal::GetBoardInfo::Request& req, spinal::GetBoardInfo::Response& res) +{ + //TODO: Bad implementation. This features should not be located in servo interface. + spinal::BoardInfo& board = board_info_res_.boards[0]; + board.imu_send_data_flag = 1; + board.dynamixel_ttl_rs485_mixed = servo_handler_.getTTLRS485Mixed(); + board.slave_id = 0; + for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) { + const ServoData& s = servo_handler_.getServo()[i]; + board.servos[i].id = s.id_; + board.servos[i].p_gain = s.p_gain_; + board.servos[i].i_gain = s.i_gain_; + board.servos[i].d_gain = s.d_gain_; + board.servos[i].profile_velocity = s.profile_velocity_; + board.servos[i].current_limit = s.current_limit_; + board.servos[i].send_data_flag = s.send_data_flag_; + board.servos[i].external_encoder_flag = s.external_encoder_flag_; + board.servos[i].joint_resolution = s.joint_resolution_; + board.servos[i].servo_resolution = s.servo_resolution_; + } + res = board_info_res_; +} + void DirectServo::jointProfilesCallback(const spinal::JointProfiles& joint_prof_msg) { for(unsigned int i = 0; i < joint_prof_msg.joints_length; i++){ diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h index 93f836d1c..a5aecde04 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h @@ -16,6 +16,7 @@ includes ------------------------------------------------------------------*/ #include #include #include +#include #include #include #include @@ -39,7 +40,8 @@ class DirectServo joint_profiles_sub_("joint_profiles", &DirectServo::jointProfilesCallback,this), servo_state_pub_("servo/states", &servo_state_msg_), servo_torque_state_pub_("servo/torque_states", &servo_torque_state_msg_), - servo_config_srv_("direct_servo_config", &DirectServo::servoConfigCallback, this) + servo_config_srv_("direct_servo_config", &DirectServo::servoConfigCallback, this), + board_info_srv_("get_board_info", &DirectServo::boardInfoCallback,this) { } ~DirectServo(){} @@ -49,6 +51,7 @@ class DirectServo void sendData(); void torqueEnable(const std::map& servo_map); void setGoalAngle(const std::map& servo_map, uint8_t value_type = 0); + DynamixelSerial& getServoHnadler() {return servo_handler_;} uint32_t rad2Pos(float angle, float scale, uint32_t zero_point_pos){ return static_cast(angle /scale + zero_point_pos); @@ -64,9 +67,11 @@ class DirectServo ros::Publisher servo_torque_state_pub_; ros::ServiceServer servo_config_srv_; + ros::ServiceServer board_info_srv_; spinal::ServoStates servo_state_msg_; spinal::ServoTorqueStates servo_torque_state_msg_; + spinal::GetBoardInfo::Response board_info_res_; uint32_t servo_last_pub_time_; uint32_t servo_torque_last_pub_time_; @@ -76,6 +81,7 @@ class DirectServo void jointProfilesCallback(const spinal::JointProfiles& joint_prof_msg); void servoConfigCallback(const spinal::SetDirectServoConfig::Request& req, spinal::SetDirectServoConfig::Response& res); + void boardInfoCallback(const spinal::GetBoardInfo::Request& req, spinal::GetBoardInfo::Response& res); /* Servo state */ struct ServoState{ diff --git a/aerial_robot_nerve/spinal/src/spinal/board_configurator.py b/aerial_robot_nerve/spinal/src/spinal/board_configurator.py index 8468f9974..6553a8cd7 100644 --- a/aerial_robot_nerve/spinal/src/spinal/board_configurator.py +++ b/aerial_robot_nerve/spinal/src/spinal/board_configurator.py @@ -34,6 +34,7 @@ def __init__(self, context): self.get_board_info_client_ = rospy.ServiceProxy(robot_ns + '/get_board_info', GetBoardInfo) self.set_board_config_client_ = rospy.ServiceProxy(robot_ns + '/set_board_config', SetBoardConfig) + self.direct_servo_config_client_ = rospy.ServiceProxy(robot_ns + '/direct_servo_config', SetDirectServoConfig) self.servo_torque_pub_ = rospy.Publisher(robot_ns + '/servo/torque_enable', ServoTorqueCmd, queue_size = 1) from argparse import ArgumentParser @@ -162,16 +163,16 @@ def treeClickedCallback(self): row = self._widget.boardInfoTreeView.currentIndex().row() param = self._widget.boardInfoTreeView.currentIndex().sibling(row, 0).data() value = self._widget.boardInfoTreeView.currentIndex().sibling(row, 1).data() - board_param_list = ['board_id', 'imu_send_data_flag', 'dynamixel_ttl_rs485_mixed'] servo_param_list = ['pid_gain', 'profile_velocity', 'current_limit', 'send_data_flag', 'external_encoder_flag', 'resolution[joint:servo]'] - if value and (param in servo_param_list): + raw_servo_id = self._widget.boardInfoTreeView.currentIndex().sibling(0,1).data() servo_index = self._widget.boardInfoTreeView.currentIndex().parent().data() board_id = self._widget.boardInfoTreeView.currentIndex().parent().parent().parent().data() self._widget.paramLabel.setText('board_id: ' + board_id + ' servo_index: ' + servo_index + ' ' + param) self._board_id = board_id self._servo_index = servo_index + self._raw_servo_id = raw_servo_id self._command = param self._current_servo_serial_index = int(self._widget.boardInfoTreeView.currentIndex().parent().child(1, 1).data()) elif value and (param in board_param_list): @@ -183,17 +184,25 @@ def treeClickedCallback(self): self._board_id = None self._command = None self._servo_index = None + self._raw_servo_id = None self._widget.paramLabel.setText('') def configureButtonCallback(self): - req = SetBoardConfigRequest() - if self._board_id: - req.data.append(int(self._board_id)) - else: + req = None + spinal_flag = False + if self._board_id == None: rospy.logerr("board id is not registered") return + elif self._board_id == '0': + spinal_flag = True + req = SetDirectServoConfigRequest() + else: + req = SetBoardConfigRequest() + req.data.append(int(self._board_id)) if self._command == 'board_id': + if spinal_flag: + return try: req.data.append(int(self._widget.lineEdit.text())) except ValueError as e: @@ -201,6 +210,8 @@ def configureButtonCallback(self): return req.command = req.SET_SLAVE_ID elif self._command == 'imu_send_data_flag': + if spinal_flag: + return try: req.data.append(distutils.util.strtobool(self._widget.lineEdit.text())) except ValueError as e: @@ -209,7 +220,10 @@ def configureButtonCallback(self): req.command = req.SET_IMU_SEND_FLAG elif self._command == 'pid_gain': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) pid_gains = list(map(lambda x: int(x), self._widget.lineEdit.text().split(','))) if len(pid_gains) != 3: raise ValueError('Input 3 gains(int)') @@ -220,7 +234,10 @@ def configureButtonCallback(self): req.command = req.SET_SERVO_PID_GAIN elif self._command == 'profile_velocity': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) req.data.append(int(self._widget.lineEdit.text())) except ValueError as e: print(e) @@ -228,7 +245,10 @@ def configureButtonCallback(self): req.command = req.SET_SERVO_PROFILE_VEL elif self._command == 'send_data_flag': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) req.data.append(distutils.util.strtobool(self._widget.lineEdit.text())) except ValueError as e: print(e) @@ -236,7 +256,10 @@ def configureButtonCallback(self): req.command = req.SET_SERVO_SEND_DATA_FLAG elif self._command == 'current_limit': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) req.data.append(int(self._widget.lineEdit.text())) except ValueError as e: print(e) @@ -257,7 +280,10 @@ def configureButtonCallback(self): req.command = req.SET_DYNAMIXEL_TTL_RS485_MIXED elif self._command == 'external_encoder_flag': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) req.data.append(distutils.util.strtobool(self._widget.lineEdit.text())) except ValueError as e: print(e) @@ -265,7 +291,10 @@ def configureButtonCallback(self): req.command = req.SET_SERVO_EXTERNAL_ENCODER_FLAG elif self._command == 'resolution[joint:servo]': try: - req.data.append(int(self._servo_index)) + if(spinal_flag): + req.data.append(int(self._raw_servo_id)) + else: + req.data.append(int(self._servo_index)) resolutions = list(map(lambda x: int(x), self._widget.lineEdit.text().split(':'))) if len(resolutions) != 2: raise ValueError('Input 2 resoultion(int)') @@ -280,9 +309,16 @@ def configureButtonCallback(self): rospy.loginfo('command: ' + str(req.command)) rospy.loginfo('data: ' + str(req.data)) try: - res = self.set_board_config_client_(req) + res = None; + if(spinal_flag): + res = self.direct_servo_config_client_(req) + else: + res = self.set_board_config_client_(req) rospy.loginfo(bool(res.success)) rospy.sleep(1) self.updateButtonCallback() except rospy.ServiceException as e: - print("/set_board_config service call failed: {}".format(e)) + if(spinal_flag): + print("/direct_servo_config service call failed: {}".format(e)) + else: + print("/set_board_config service call failed: {}".format(e)) diff --git a/aerial_robot_nerve/spinal/src/spinal/servo_monitor.py b/aerial_robot_nerve/spinal/src/spinal/servo_monitor.py index 26a7e1dd7..f5e840cc2 100644 --- a/aerial_robot_nerve/spinal/src/spinal/servo_monitor.py +++ b/aerial_robot_nerve/spinal/src/spinal/servo_monitor.py @@ -43,6 +43,7 @@ def __init__(self, context): self.get_board_info_client_ = rospy.ServiceProxy(robot_ns + '/get_board_info', GetBoardInfo) self.set_board_config_client_ = rospy.ServiceProxy(robot_ns + '/set_board_config', SetBoardConfig) + self.set_direct_servo_config_client_ = rospy.ServiceProxy(robot_ns + '/direct_servo_config', SetDirectServoConfig) self.servo_torque_pub_ = rospy.Publisher(robot_ns + '/servo/torque_enable', ServoTorqueCmd, queue_size = 1) from argparse import ArgumentParser @@ -171,11 +172,17 @@ def jointCalib(self): if servo_index == -1: rospy.logerr("No servo exists") return - - req = SetBoardConfigRequest() - req.data.append(int(self._widget.servoTableWidget.item(servo_index, self._headers.index("board")).text())) #board id - req.data.append(int(self._widget.servoTableWidget.item(servo_index, self._headers.index("index")).text())) #servo index - + + board_id = int(self._widget.servoTableWidget.item(servo_index, self._headers.index("board")).text()) + servo_id = int(self._widget.servoTableWidget.item(servo_index, self._headers.index("index")).text()) + req = None + if(board_id == 0): + req = SetDirectServoConfigRequest() + req.data.append(servo_id) + else: + req = SetBoardConfigRequest() + req.data.append(board_id) + req.data.append(servo_id) try: req.data.append(int(self._widget.homingOffsetLineEdit.text())) except ValueError as e: @@ -194,19 +201,25 @@ def jointCalib(self): rospy.loginfo('command: ' + str(req.command)) rospy.loginfo('data: ' + str(req.data)) try: - res = self.set_board_config_client_(req) + if(board_id == 0): + res = self.set_direct_servo_config_client_(req) + else: + res = self.set_board_config_client_(req) rospy.loginfo(bool(res.success)) except rospy.ServiceException as e: print("/set_board_config service call failed: %s"%e) def boardReboot(self): servo_index = self._widget.servoTableWidget.currentIndex().row() + board_id = int(self._widget.servoTableWidget.item(servo_index, self._headers.index("board")).text()) if servo_index == -1: rospy.logerr("No servo exists") return - + if board_id == 0: + rospy.logerr("Spinal cannot be rebooted from rqt") + return req = SetBoardConfigRequest() - req.data.append(int(self._widget.servoTableWidget.item(servo_index, self._headers.index("board")).text())) #board id + req.data.append(board_id) #board id req.command = req.REBOOT rospy.loginfo('published message')