def callback_chi(msg): state = ChainState() state.chi = msg.data state.chi_desired = msg.data state.poses = fk(msg.data) drawer.set_state(state) redraw()
# main # rospy.init_node('chain_viz') global redraw_flag redraw_flag = True drawer = ChainDrawer() drawer.set_info(info) state = ChainState() state.chi = [0.0]*6 state.chi_desired = [0.0]*6 state.weights = [1.0]*6 state.poses = [kdl.Frame()] drawer.set_state(state) def callback_pose(msg): drawer.set_pose(msg) redraw() def callback_end_pose(msg): global state state.poses = [pm.fromMsg(msg.pose)] drawer.set_state(state) def callback_chi(msg):