Skip to content

Commit

Permalink
[Spinal][Servo] workaround to use rqt to monitor and setup direct ser…
Browse files Browse the repository at this point in the history 
…vos.
  • Loading branch information
@sugihara-16
sugihara-16 committed Jun 4, 2024
1 parent 8541587 commit d787196
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 25 deletions.
42 changes: 40 additions & 2 deletions aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,19 +20,23 @@ 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;
servo_state_msg_.servos = new spinal::ServoState[MAX_SERVO_NUM];
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()
Expand Down Expand Up @@ -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<uint8_t, float>& servo_map)
Expand Down Expand Up @@ -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++){
Expand Down
8 changes: 7 additions & 1 deletion aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/servo/servo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ includes ------------------------------------------------------------------*/
#include <spinal/ServoTorqueStates.h>
#include <spinal/ServoTorqueCmd.h>
#include <spinal/SetDirectServoConfig.h>
#include <spinal/GetBoardInfo.h>
#include <spinal/JointProfiles.h>
#include <string.h>
#include <config.h>
Expand All @@ -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(){}
Expand All @@ -49,6 +51,7 @@ class DirectServo
void sendData();
void torqueEnable(const std::map<uint8_t, float>& servo_map);
void setGoalAngle(const std::map<uint8_t, float>& 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<uint32_t>(angle /scale + zero_point_pos);
Expand All @@ -64,9 +67,11 @@ class DirectServo
ros::Publisher servo_torque_state_pub_;

ros::ServiceServer<spinal::SetDirectServoConfig::Request, spinal::SetDirectServoConfig::Response, DirectServo> servo_config_srv_;
ros::ServiceServer<spinal::GetBoardInfo::Request, spinal::GetBoardInfo::Response, DirectServo> 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_;
Expand All @@ -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{
Expand Down
64 changes: 50 additions & 14 deletions aerial_robot_nerve/spinal/src/spinal/board_configurator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand All @@ -183,24 +184,34 @@ 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:
print(e)
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:
Expand All @@ -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)')
Expand All @@ -220,23 +234,32 @@ 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)
return
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)
return
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)
Expand All @@ -257,15 +280,21 @@ 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)
return
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)')
Expand All @@ -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))
29 changes: 21 additions & 8 deletions aerial_robot_nerve/spinal/src/spinal/servo_monitor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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')
Expand Down

0 comments on commit d787196

Please sign in to comment.