def __init__(self): rospy.init_node('robot_state', anonymous=False) self.pub = rospy.Publisher('joint_states', JointState, queue_size=10) self.msg_joint_state = JointState() self.msg_joint_state.name = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'irbp_a250/joint2', 'irbp_a250/joint4' ] self.msg_joint_state.position = [0, 0, 0, 0, 0, 0, 0, 0] robot_ip = rospy.get_param('~robot_ip', '192.168.30.4') j23_coupled = rospy.get_param('~J23_coupled', True) self.logger_robot = LoggerRobot() self.logger_robot.connect(robot_ip) self.talker(j23_coupled)
class NdRobotLogger(): def __init__(self): rospy.init_node('robot_state', anonymous=False) self.pub = rospy.Publisher( 'joint_states', JointState, queue_size=10) self.msg_joint_state = JointState() self.msg_joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'irbp_a250/joint2', 'irbp_a250/joint4'] self.msg_joint_state.position = [0, 0, 0, 0, 0, 0, 0, 0] robot_ip = rospy.get_param('~robot_ip', '192.168.30.4') j23_coupled = rospy.get_param('~J23_coupled', True) self.logger_robot = LoggerRobot() self.logger_robot.connect(robot_ip) self.talker(j23_coupled) def talker(self, j23_coupled=True): while not rospy.is_shutdown(): if not self.logger_robot.read_raw_logger(): break if len(self.logger_robot.float_joints) > 0: self.msg_joint_state.header.stamp = rospy.Time.now() joints_pose = self.logger_robot.float_joints.popleft() for i in range(0, len(joints_pose)): #len(joints_pose)-3 self.msg_joint_state.position[i] = ( np.deg2rad(joints_pose[i])) if j23_coupled: self.msg_joint_state.position[2] += ( -1 * self.msg_joint_state.position[1]) #rospy.loginfo(len(logger_robot.joints)) self.pub.publish(self.msg_joint_state) rospy.sleep(0.01) self.logger_robot.disconnect()
def __init__(self): rospy.init_node('robot_state', anonymous=False) self.pub = rospy.Publisher( 'joint_states', JointState, queue_size=10) self.msg_joint_state = JointState() self.msg_joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'irbp_a250/joint2', 'irbp_a250/joint4'] self.msg_joint_state.position = [0, 0, 0, 0, 0, 0, 0, 0] robot_ip = rospy.get_param('~robot_ip', '192.168.30.4') j23_coupled = rospy.get_param('~J23_coupled', True) self.logger_robot = LoggerRobot() self.logger_robot.connect(robot_ip) self.talker(j23_coupled)
class NdRobotLogger(): def __init__(self): rospy.init_node('robot_state', anonymous=False) self.pub = rospy.Publisher('joint_states', JointState, queue_size=10) self.msg_joint_state = JointState() self.msg_joint_state.name = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'irbp_a250/joint2', 'irbp_a250/joint4' ] self.msg_joint_state.position = [0, 0, 0, 0, 0, 0, 0, 0] robot_ip = rospy.get_param('~robot_ip', '192.168.30.4') j23_coupled = rospy.get_param('~J23_coupled', True) self.logger_robot = LoggerRobot() self.logger_robot.connect(robot_ip) self.talker(j23_coupled) def talker(self, j23_coupled=True): while not rospy.is_shutdown(): self.logger_robot.read_raw_logger() if len(self.logger_robot.float_joints) > 0: self.msg_joint_state.header.stamp = rospy.Time.now() joints_pose = self.logger_robot.float_joints.popleft() for i in range(0, len(joints_pose)): #len(joints_pose)-3 self.msg_joint_state.position[i] = (np.deg2rad( joints_pose[i])) if j23_coupled: self.msg_joint_state.position[2] += ( -1 * self.msg_joint_state.position[1]) #rospy.loginfo(len(logger_robot.joints)) self.pub.publish(self.msg_joint_state) rospy.sleep(0.01) self.logger_robot.disconnect()