def robot_state_publisher(self):
if self.current_robot_status['ready']:
state_num = 0
if self.current_robot_status['busy']:
state_num = 1
if self.current_robot_status['direct_teaching']:
state_num = 2
if self.current_robot_status['collision']:
state_num = 3
if self.current_robot_status['emergency']:
state_num = 4
status_msg = GoalStatusArray()
status_msg.header.stamp = rospy.Time.now()
status = GoalStatus()
status.goal_id.stamp = rospy.Time.now()
status.goal_id.id = ""
status.status = state_num
status.text = ROBOT_STATE[state_num]
status_msg.status_list = [status]
self.indy_state_pub.publish(status_msg)
def make_goal_array(self):
for path in range(0, len(self.path_points) - 1):
new_goal = GoalStatus()
new_goal.goal_id.id = str(path)
new_goal.status = 0
new_goal.text = 'PENDING'
self.goal_arr.status_list.append(new_goal)
def goal_status_array(cls, msg, obj):
obj.header = decode.header(msg, obj.header, "")
for i in range(msg['_length']):
goal_status = GoalStatus()
goal_status.goal_id.stamp = decode.time_stamp(msg,
goal_id.stamp, i)
goal_status.goal_id.id = msg['%s_id' % i]
goal_status.status = msg['%s_status' % i]
goal_status.text = msg['%s_text' % i]
obj.status_list.append(goal_status)
return(obj)
