iory / rcb4

MIT License
1 stars 8 forks source link

Add new try-catch in publish_joint_states() #80

Closed 708yamaguchi closed 1 month ago

708yamaguchi commented 1 month ago

This PR catch the following error, which may occur when plugging and unplugging kondoh7.

Oct 21 22:44:56 plucky-moonshadow user.service[2944]: Traceback (most recent call last):    
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/devel/lib/kxr_controller/rcb4_ros_bridge.py", line 15, in <module>
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     exec(compile(fh.read(), python_script, 'exec'), context)
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py", line 776, in <module>
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     ros_bridge.run()                     
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py", line 758, in run
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     self.publish_joint_states()                                                                                                                      
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py", line 724, in publish_joint_states
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     av = self.interface.angle_vector()
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/rcb4/armh7interface.py", line 573, in angle_vector
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     self._angle_vector()[all_servo_ids], all_servo_ids)
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/rcb4/armh7interface.py", line 538, in _angle_vector
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     return self.read_cstruct_slot_vector(
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:   File "/home/rock/ros/kxr/src/rcb4/rcb4/armh7interface.py", line 434, in read_cstruct_slot_vector
Oct 21 22:44:56 plucky-moonshadow user.service[2944]:     return np.frombuffer(b, dtype=c_type_to_numpy_format(c_type))
Oct 21 22:44:56 plucky-moonshadow user.service[2944]: ValueError: buffer size must be a multiple of element size