EuRoC Forum

[AIS]

Hello,

I have performed some runs of Task 2 with the new update and it seems that there is a problem with the reset of the workpiece when a robot-workpiece collision occurs. After the collision the workpiece is reset and when the robot goes to the new /hole_guess there is no hole there. I have checked the axis of /hole_guess in Rviz and the robot is correctly placed but there is no hole there.

I don't know if the information on /hole_guess is from the previous position of the workpiece.

Regards

[ZdenekM]

I can confirm this issue.

[ipa-paq]

Hello,

We found a posible solution. The problem could be that TF doesn´t have time enough to update the new position before generating /hole_guess. Please could you add "time.sleep(5)" to line 543 (inside if self.t2_workpiece_collision:) in /ipa325_euroc_sim/ipa325_com_server/scripts/ipa325_com_server.py and tell us if now the problem persists? If still persits could you send us a image capture showing the new /hole_guess after colliding? Thank you.

Regards,
Pablo Quilez

[dti]

Hi Pablo

Inserting a small delay solves the problem for us. Thanks!

[ipa-paq]

You are welcome.

[AIS]

If I add the sleep in line 543 the simulation con not finish the initialization and gets freezed and the /execute_task service is never available. In my case with the screen saying:

**************************************************************
* SUCCESS *
* SIMULATION RUNNING *
**************************************************************

publishing simulation_running
publishing and latching message. Press ctrl-C to terminate

[ipa-paq]

Hello AIS,

Could your problem be that the sleep is not inside "if" indented? If it were at a different level, this could freeze the loop. I copy down the updated version of the file. If the problem persists I will try to think another solution. Thank you!

Code: Select all

#!/usr/bin/env python
'''
Created on Jun 23, 2014

@author: Bernd Winkler
'''

import os
import shutil

from subprocess32 import Popen
import datetime

import traceback

import numpy as np
import math

import rospy
import tf

import actionlib
import std_msgs.msg
import geometry_msgs.msg
import std_srvs.srv

# for robot control
# import ipa325_msgs.msg
import control_msgs.msg
import trajectory_msgs.msg

from ipa325_com_msgs.srv import *  # SolveTask, InitTask, ExecuteTask, ExecuteTaskResponse
from ipa325_com_msgs.msg import SolveTaskAction, SolveTaskGoal

current_dir = os.path.dirname(__file__)
gazebo_human_move_path = os.path.abspath(current_dir + '/../../ipa325_euroc_human_move/scripts')
os.sys.path.append(gazebo_human_move_path)
from gazebo_human_move import gazebo_human_move as human_mover

from gazebo_msgs.srv import GetModelState, SetModelState, SetModelStateRequest, GetLinkState, SetLinkState
# import ipa325_gazebo_plugins.msg
import sensor_msgs.msg

import glob
import time

import atexit

import threading
import random

import subprocess32

param_bagfile_base_path = '~/euroc_track1_logging/'
param_run_as_test = None

def gazebo_get_modelstate(model_name, relative_entity_name):
   service_name = '/gazebo/get_model_state'
   rospy.wait_for_service(service_name)
   try:
      service = rospy.ServiceProxy(service_name, GetModelState)
      resp = service(model_name, relative_entity_name)
      return resp
   except rospy.ServiceException, e:
      rospy.logerr("Service call failed: %s" % e)

def gazebo_set_modelstate(model_name, relative_entity_name, ms):
   service_name = '/gazebo/set_model_state'
   rospy.wait_for_service(service_name)
   try:
      service = rospy.ServiceProxy(service_name, SetModelState)

      resp = service(ms)
      return resp
   except rospy.ServiceException, e:
      rospy.logerr("Service call failed: %s" % e)

def gazebo_get_linkstate(name, reference_name):
   service_name = '/gazebo/get_link_state'
   service_type = GetLinkState

   rospy.wait_for_service(service_name)
   try:
      service = rospy.ServiceProxy(service_name, service_type)
      resp = service(name, reference_name)
      return resp
   except rospy.ServiceException, e:
      rospy.logerr("Service call failed: %s" % e)

def gazebo_set_linkstate(name, reference_name, linkstate):
   service_name = '/gazebo/set_link_state'
   service_type = SetLinkState

   rospy.wait_for_service(service_name)
   try:
      service = rospy.ServiceProxy(service_name, service_type)
      resp = service(linkstate)
      return resp
   except rospy.ServiceException, e:
      rospy.logerr("Service call failed: %s" % e)


simulation_environment = None
bag_logger = None

class com_server(object):
   active_task = ""
   active_task_topic = None

   task_ids = ['1.1', '1.2', '1.3', '2']
   task_time_budget_s = {'1.1':0, '1.2':30, '1.3':30, '2':600}

   t2_param_insertion_depth = 0.006  # tcp has to be 6mm into workpiece surface to

   general_log_topics = ['tf', 'rosout', 'active_task', 'time_budget']

   tf_frames_lock = threading.Lock()
   do_tf_broadcast_thread = None
   tf_frames_dict = {"/hole_guess":([0.0, -0.61, 0.5], [0.3, 0.3, 0.3, 0]),
               "/rivet_home":([0.0, -0.61, 0.5], [0.3, 0.3, 0.3, 0])}

   tf_listener = None
   client_ur_joint = None

   # complete list of hole names
   hole_tf_framenames = ['h0', 'h1', 'h2', 'h3', 'h4', 'h5', 'h6', 'h7', 'h8', 'h9', 'h10', 'h11', 'h12', 'h13', 'h14', 'h15', 'h16', 'h17', 'h18', 'h19', 'h20', 'h21', 'h22', 'h23', 'h24', 'h25', 'h26', 'h27', 'h28', 'h29', 'h30', 'h31', 'h32', 'h33']
   t1_1_trajectory_count = 20
   timestep = 1  # time countdown in 10 s steps

   t2_sm_lock = threading.Lock()
   t2_sm_state = ""
   t2_cur_hole_tf_framename = None
   t2_run_human_interference = None
   t2_topic_target_frame = None

   t0_pub_timebudget = None

   tf_broadcaster_pointer_lock = threading.Lock()
   tf_broadcaster_pointer = tf.TransformBroadcaster()

   def init_for_test(self):
      rospy.loginfo("initializing for test")
      self.task_time_budget_s = {'1.1':0, '1.2':10, '1.3':30 , '2':200}
      self.task_ids = ['1.1', '1.2', '1.3', '2']
      self.t1_1_trajectory_count = 3
      # dont move to any real hole but to hole_guess
      self.hole_tf_framenames = ['hole_guess']
      pass

   def home_robot(self):
      # Todo: test if robot is already there
      rospy.loginfo("moving robot to home position")
      goal = control_msgs.msg.FollowJointTrajectoryGoal()
      goal.trajectory.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
      p = trajectory_msgs.msg.JointTrajectoryPoint()
      p.positions = [0.1, -math.pi / 2 + 0.1, 0.1, -math.pi / 2 + 0.1, 0.1, 0.1]
      p.time_from_start = rospy.Duration(1)
      goal.trajectory.points.append(p)

      # Send FoJoTra goal
      # rospy.loginfo("sending goal to robot")
      self.client_ur_joint.send_goal(goal)
      self.client_ur_joint.wait_for_result()
      rospy.loginfo("robot in home position")

   def do_tf_broadcast(self):
      try:
         tf_broadcaster = tf.TransformBroadcaster()
         rate = rospy.Rate(50.0)
         while not rospy.is_shutdown():
            with self.tf_frames_lock:
               for frame_name in self.tf_frames_dict:
                  # publish tf frames
                  # rospy.loginfo("publishing " + frame_name)
                  tf_broadcaster.sendTransform(*self.tf_frames_dict[frame_name], time=rospy.Time.now(), child=frame_name, parent="/ur_base")
            rate.sleep()
      except:
         traceback.print_exc()
         pass

      rospy.loginfo("leaving thread do_tf_broadcast")

   def cb_range_to_tf_frame(self, msg):
      ranges = msg.ranges
      dist = ranges[2]
      with self.tf_broadcaster_pointer_lock:
         self.tf_broadcaster_pointer.sendTransform(translation=(dist, 0, 0), rotation=(1, 0, 0, 0), time=rospy.Time.now(), child="pointed_to", parent="/human_arm_r_pointer")


   def __init__(self):
      atexit.register(self._atexit)

      # use special setup for testing
      if param_run_as_test:
         self.init_for_test()

      # paths used
      directory = os.path.expanduser(param_bagfile_base_path)
      # make sure it exists
      if not os.path.exists(directory):
         os.makedirs(directory)
         if not os.path.exists(directory):
            rospy.logerr("unable to create directory for logging " + directory)

      self.logging_dir = directory
      rospy.loginfo("log files will be written to " + directory)

      self.tf_listener = tf.TransformListener()

      self.sub_pointer = rospy.Subscriber("pointer", sensor_msgs.msg.LaserScan , self.cb_range_to_tf_frame)
      self.sub_collision_human = rospy.Subscriber("collision_human_robot", std_msgs.msg.Bool , self.cb_on_human_robotmoving_collision)
      self.sub_collision_workpiece = rospy.Subscriber("/gazebo/collisions/ur5_workpiece", std_msgs.msg.Bool , self.cb_on_robot_workpiece_collision)

      # register converter (tf->topic) for workpiece pose
      self.t1_1_sub_wp_pose = rospy.Subscriber("t1_2_workpiece", geometry_msgs.msg.PoseStamped, self.t1_2_cb_workpiece_pose_converter)
      self.t1_1_pub_wp_pose_gt = rospy.Publisher("t1_2_workpiece_gt", geometry_msgs.msg.PoseStamped)

      self.t1_3_sub_pointer_pose = rospy.Subscriber("t1_3_pointedpose", geometry_msgs.msg.PoseStamped, self.t1_3_cb_pointed_pose_converter)
      self.t1_3_pub_pointer_pose_gt = rospy.Publisher("t1_3_pointedpose_gt", geometry_msgs.msg.PoseStamped)

      self.t2_pub_hole_pose_gt = rospy.Publisher("t2_holepose_gt", geometry_msgs.msg.PoseStamped)
      self.t2_pub_tcp_pose = rospy.Publisher("t2_tcppose", geometry_msgs.msg.PoseStamped)


      rospy.loginfo("waiting for simulator")
      s = rospy.wait_for_message("simulation_running", std_msgs.msg.String)
      if s == None:
         rospy.logerr("SIMULATOR HAS TO BE STARTED EXTERNALLY!!! DOESNT APPEAR SO RIGHT NOW, BUT LETS SEE :-)")

      # get control of the scene
      self.client_ur_joint = actionlib.SimpleActionClient('/simulation_ur5/joint_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
      if not self.client_ur_joint.wait_for_server():
         rospy.logerr("client_ur_joint: actionclient not found")
         raise Exception()

      self.reset_simulator()
      time.sleep(4)  # let settle


      # backup initial human position
      scoped_link_name = "human"
      relative_entity_name = "world"
      # get current workpiece pose
      resp = gazebo_get_modelstate(scoped_link_name, relative_entity_name)
      ms_req = SetModelStateRequest()
      # Set Gazebo Model pose and twist
      # string model_name         # model to set state (pose and twist)
      # geometry_msgs/Pose pose    # desired pose in reference frame
      # geometry_msgs/Twist twist   # desired twist in reference frame
      # string reference_frame     # set pose/twist relative to the frame of this entity (Body/Model)
      #                       # leave empty or "world" or "map" defaults to world-frame


      ms_req.model_state.model_name = scoped_link_name
      ms_req.model_state.reference_frame = relative_entity_name
      ms_req.model_state.pose = resp.pose
      ms_req.model_state.twist = resp.twist
      self.gz_modelstate_human_init = ms_req.model_state

      # backup workpiece initial pose
      self.gz_workpiecepose_initial = self.get_workpiece_pose()

      self.do_tf_broadcast_thread = threading.Thread(target=self.do_tf_broadcast)
      self.do_tf_broadcast_thread.start()

      # start topics
      self.active_task_topic = rospy.Publisher('active_task', std_msgs.msg.String, latch=True)
      self.active_task_topic.publish("0")

      self.t0_pub_timebudget = rospy.Publisher('time_budget', std_msgs.msg.Int32, latch=True)
      self.t0_pub_timebudget.publish(0)

      # start action client
      self.client = actionlib.SimpleActionClient('solve_task', SolveTaskAction)
      rospy.loginfo("waiting for client")
      self.client.wait_for_server()

      # start services
      self.t_2_service_load_rivet = rospy.Service("load_rivet", std_srvs.srv.Empty, self.cb_load_rivet)
      self.t_2_service_trigger_rivet = rospy.Service("trigger_rivet", std_srvs.srv.Empty, self.cb_trigger_rivet)

      self.service_execute_task = rospy.Service("execute_task", ExecuteTask, self.cb_execute_task)
      rospy.loginfo("service 'execute_task' ready for requests")

   def _atexit(self):
      self.stop_logging()
      try:
         self.do_tf_broadcast_thread.join(10)
      except:
         rospy.logerr("couldn't join tf broadcast thread")

   def _cancel_goal_wait(self):
      '''cancels last goal and waits until acknowledged'''
      self.client.cancel_goal()
      self.client.wait_for_result(timeout=rospy.Duration(10))

   def get_trajectory_fns(self, traj_dir_pattern="/trajectories_p?/trajectory_*.yaml"):
      module_path = __file__  # path of current module
      dirname_cur = os.path.dirname(module_path)
      dirname_traj = dirname_cur + "/../../ipa325_euroc_human_move/yaml"
      dirname_traj = os.path.normpath(dirname_traj)
      glob_pattern = dirname_traj + traj_dir_pattern

      traj_pathfns = glob.glob(glob_pattern)
      if len(traj_pathfns) == 0:
         raise Exception("on trajectories found using pattern " + glob_pattern)

      rospy.loginfo("used pattern for traj glob: %s" % glob_pattern)
      return traj_pathfns

   def init_task_caller(self, taskid):
      rospy.loginfo("asking client to prepare for task" + taskid)
      found = rospy.wait_for_service('/init_task')
      init_task_service = rospy.ServiceProxy('/init_task', InitTask)
      init_task_service(taskid)
      rospy.loginfo('preparing for task ' + taskid + ' done')

   def get_workpiece_pose(self):
      r"""
      return current workpiece pose as link state
      """
      scoped_link_name = "workpiece::workpiece_link"
      relative_entity_name = "world"
      # get current workpiece pose
      ls_old = gazebo_get_linkstate(scoped_link_name, relative_entity_name)
      if not ls_old.success:
         raise Exception("unable to retrieve link state")

      return ls_old.link_state

   def set_workpiece_pose(self, linkstate):
      scoped_link_name = "workpiece::workpiece_link"
      relative_entity_name = "world"
      # set workpiece pose
      gazebo_set_linkstate(scoped_link_name, relative_entity_name, linkstate)


   def change_workpiece_pose(self):
      r"""
      change workpiece pose with small random noise
      """
      # get current workpiece pose
      ls_new = self.get_workpiece_pose()
      # add noise
      ls_new.pose.position.x += random.uniform(-0.03, 0.03)
      ls_new.pose.position.y += random.uniform(-0.03, 0.03)
      ls_new.pose.position.z += 0.0001
      q = ls_new.pose.orientation
      euler_orig = list(tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w)))
      euler_orig[2] += random.uniform(-0.2, 0.2)
      q_new = tf.transformations.quaternion_from_euler(*euler_orig)
      (ls_new.pose.orientation.x, ls_new.pose.orientation.y, ls_new.pose.orientation.z, ls_new.pose.orientation.w) = (q_new[0], q_new[1], q_new[2], q_new[3])

      # set pose
      self.set_workpiece_pose(ls_new)

      rospy.loginfo("changed pose of workpiece")

   def reset_simulator(self):
      # ## resetting gazebo world
      reset_call = rospy.ServiceProxy("/gazebo/reset_world", std_srvs.srv.Empty)
      reset_call()
      rospy.loginfo("Resetting Gazebo")
      rospy.sleep(5)
      self.home_robot()

   def start_logging(self, topics=[]):
      global bag_logger
      # determine filename to which to log

      timestamp = datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%S")
      if param_run_as_test:
         self.bag_fn = "{}_task_{}.bag".format(timestamp, self.active_task)
      else:
         self.bag_fn = "{}_task_{}.bag".format(timestamp, self.active_task)

      # determine relevant topics to log
      relevant_topics = topics + self.general_log_topics
      args = ['rosbag', 'record', '-O' + self.bag_fn] + relevant_topics + ['__name:=ipa325_record']

      rospy.loginfo("start logging of the following topics: %s" % str(relevant_topics))
      bag_logger = Popen(args)

      # wait for logging really in process
      bag_current_filepath = os.path.join(os.getcwd(), self.bag_fn)
      while not os.path.isfile(bag_current_filepath + ".active"):
         time.sleep(1)

      rospy.loginfo("logging started")
      pass

   def stop_logging(self):
      global bag_logger
      if bag_logger is not None:
         try:
            subprocess32.call(['rosnode', 'kill', 'ipa325_record'])
            rospy.loginfo("logging stopped")
         except:
            rospy.loginfo("Unexpected error:" + sys.exc_info())
            rospy.loginfo("except")
            pass

         bag_logger = None

         bag_current_filepath = os.path.join(os.getcwd(), self.bag_fn)
         if not os.path.isfile(bag_current_filepath + ".active"):
            rospy.logwarn("no bagfile created")
            return

         bag_target_filepath = os.path.join(self.logging_dir, self.bag_fn)

         cnt = 0
         max_cnt = 10
         while (not os.path.isfile(bag_current_filepath) and cnt < max_cnt):
            rospy.loginfo('Waiting for bagfile to be ready')
            # print 'Counter:', cnt
            cnt = cnt + 1
            rospy.sleep(1)

         if (cnt >= max_cnt):
            rospy.logerr('bagfile not ready at' + bag_current_filepath)
         else:
            rospy.loginfo('file found')
            shutil.move(bag_current_filepath, bag_target_filepath)

   def cleanup(self, taskid):
      # cancel the solve_task action call (don't wait for the result)
      self._cancel_goal_wait()
      rospy.loginfo('execution of task finished: ' + taskid)
      self.active_task = ""
      # stop logger
      self.stop_logging()
      self.active_task_topic.publish("0")

   def check_topics(self, topics_dict):
      '''test if the given topics are available'''
      for topicname_expected in topics_dict.keys():
         m = rospy.wait_for_message(topicname_expected, topics_dict[topicname_expected], timeout=10)
         if m == None:
            rospy.logerr("didn't find topic {} of type {} that has to be published by team".format(topicname_expected, topics_dict[topicname_expected]))
            raise Exception("topic missing")

   tf_lock = threading.Lock()
   def getcurrentposec(self, frame_from='/world', frame_to='/tcp'):
      """delivers current cartesian position (trans,rot_tf)"""
      with self.tf_lock:
         while not rospy.is_shutdown():
            try:
               return self.tf_listener.lookupTransform(frame_from, frame_to, rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
               rospy.logdebug("waiting for tf: " + frame_from + ' -> ' + frame_to)
               time.sleep(0.05)
               continue

   def get_next_hole_frame_name(self):
      self.t2_cur_hole_tf_framename = random.choice(self.hole_tf_framenames)
      rospy.loginfo("next hole's frame is " + self.t2_cur_hole_tf_framename)
      return self.t2_cur_hole_tf_framename

   def t2_statemachine(self, targetstate):
      # print targetstate
      if self.active_task != '2':
         return
      try:
         with self.t2_sm_lock:
            if targetstate == 'home':
               if self.t2_sm_state == 'hole':
                  # test if hole was reached
                  # if rivet is sufficiently deep inserted (test in z direction)
                  (trans, rot_tf) = self.getcurrentposec(self.t2_cur_hole_tf_framename, '/tcp')

                  in_hole = trans[2] < self.t2_param_insertion_depth and np.linalg.norm(trans[:2]) < 0.003  # inserted 6mm deep, xy distance of 3 mm

                  # assume we get sometimes close enough
                  if param_run_as_test:
                     in_hole = random.choice([True, False])

                  if not in_hole:
                     rospy.logwarn("rivet is not sufficiently deep inserted. please adjust.")
                     return

                  # publish hole pose and tcp pose as topic
                  rospy.logdebug("hole pose gt")

                  if param_run_as_test:
                     gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", "hole_guess")  # try to reach hole_guess
                  else:
                     gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", self.current_hole_tf_frame_name)  # try to reach real hole

                  ps = geometry_msgs.msg.PoseStamped()

                  ps.pose.position.x = gt_pos[0]
                  ps.pose.position.y = gt_pos[1]
                  ps.pose.position.z = gt_pos[2]

                  ps.pose.orientation.x = gt_rottf[0]
                  ps.pose.orientation.y = gt_rottf[1]
                  ps.pose.orientation.z = gt_rottf[2]
                  ps.pose.orientation.w = gt_rottf[3]
                  ps.header.frame_id = "workpiece_link"
                  self.t2_pub_hole_pose_gt.publish(ps)
                  rospy.sleep(0.5)

                  rospy.logdebug("tcp pose after riveting")

                  gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", "/tcp")
                  ps = geometry_msgs.msg.PoseStamped()

                  ps.pose.position.x = gt_pos[0]
                  ps.pose.position.y = gt_pos[1]
                  ps.pose.position.z = gt_pos[2]

                  ps.pose.orientation.x = gt_rottf[0]
                  ps.pose.orientation.y = gt_rottf[1]
                  ps.pose.orientation.z = gt_rottf[2]
                  ps.pose.orientation.w = gt_rottf[3]
                  ps.header.frame_id = "workpiece_link"
                  self.t2_pub_tcp_pose.publish(ps)

                  # set topic target_frame to 'rivet_home'
                  self.t2_topic_target_frame.publish('/rivet_home')

                  rospy.loginfo("reached hole position")
                  self.t2_sm_state = targetstate  # switch to next state
               pass
            elif targetstate == 'hole':
               if self.t2_sm_state == 'home':
                  # test if home was reached
                  (trans, rot_tf) = self.getcurrentposec('/rivet_home', '/tcp')
                  at_home = np.linalg.norm(trans) < 0.01  # 10 mm tolerance

                  if not at_home:
                     rospy.logwarn("not yet reached home position. please adjust.")
                     return

                  # robot reached home: test if collision occured before
                  if self.t2_workpiece_collision:
                     # reset workpiece position
                     self.set_workpiece_pose(self.gz_workpiecepose_initial)
                     # plus noise
                     self.change_workpiece_pose()
                     self.t2_workpiece_collision = False
                     time.sleep(5)

                  # move tf frame hole_guess to hole + limited random offset
                  self.current_hole_tf_frame_name = self.get_next_hole_frame_name()
                  (gt_trans, gt_rot_tf) = self.getcurrentposec('/ur_base', self.current_hole_tf_frame_name)

                  new_trans = np.asarray(gt_trans) + np.random.uniform(-0.003, 0.003, 3)  # +-3m mm offset possible
                  new_rot_tf = np.asarray(gt_rot_tf) * random.uniform(0, 0.1)  # some rotational noise as well

                  with self.tf_frames_lock:
                     self.tf_frames_dict['/hole_guess'] = (list(new_trans), list(new_rot_tf))

                  # set topic target_frame to 'hole_guess'
                  self.t2_topic_target_frame.publish('/hole_guess')
                  rospy.loginfo("reached home position")

                  self.t2_sm_state = targetstate
               pass

            elif targetstate == 'collision_workpiece':
               rospy.logerr("workpiece/table-robot collision detected")
               # set topic target_frame to 'rivet_home'
               self.t2_workpiece_collision = True
               # start over from  home
               self.t2_topic_target_frame.publish('/rivet_home')
               self.t2_sm_state = 'home'  # switch to next state
               pass

            elif targetstate == 'collision_human':
               rospy.loginfo("harmfull human-robot collision detected")
               # move human out of the way
               scoped_link_name = "human::human_base_link"
               relative_entity_name = "world"
               # TODO switch off human
               # reset human to initial pose
               gazebo_set_modelstate(scoped_link_name, relative_entity_name, self.gz_modelstate_human_init)
               time.sleep(3)
               # set topic target_frame to 'rivet_home'
               self.t2_topic_target_frame.publish('/rivet_home')
               self.t2_sm_state = 'home'  # switch to next state
               pass

            elif targetstate == 'init':
               # set topic target_frame to 'rivet_home'
               self.t2_topic_target_frame.publish('/rivet_home')
               self.t2_sm_state = 'home'  # switch to next state
            else:
               rospy.logerr("state change not expected {} -> {}".format(self.t2_sm_state, targetstate))
               raise Exception()
      except:
         traceback.print_exc()

   def cb_load_rivet(self, req):
      rospy.logdebug("service load_rivet called")
      resp = std_srvs.srv.EmptyResponse()
      if self.active_task != '2':
         rospy.logwarn("task 2 is not active. skipping call")
         return resp

      self.t2_statemachine('hole')
      return resp

   def cb_trigger_rivet(self, req):
      rospy.logdebug("service trigger_rivet called")
      resp = std_srvs.srv.EmptyResponse()
      if self.active_task != '2':
         rospy.logwarn("task 2 is not active. skipping call")
         return resp

      self.t2_statemachine('home')
      return resp


   def cb_on_human_robotmoving_collision(self, msg):
      '''gets called when robot induced collision between human and robot happens'''
      hit = msg.data
      if hit:
         self.t2_statemachine('collision_human')
         pass

   def cb_on_robot_workpiece_collision(self, msg):
      '''gets called when robot collides with table or workpiece'''
      hit = msg.data
      if hit:
         self.t2_statemachine('collision_workpiece')
         pass

   def do_t2_human_interference(self):
      '''launch human interference at random times'''
      try:
         fns = self.get_trajectory_fns()
         rospy.loginfo("found {} trajectories files".format(str(fns)))

         while (self.active_task == '2') and not rospy.is_shutdown() and self.t2_run_human_interference:
            time.sleep(random.uniform(5, 30))
            human_mover(random.choice(fns), reset_world=False)
      except:
         traceback.print_exc()

   def t1_2_cb_workpiece_pose_converter(self, data):
      ''' republish tf frame in normal topic as PoseStamped'''

      rospy.logdebug("received workpiece pose from team")
      # received wp pose from team
      # extract wp_gt pose from tf
      wp_gt_pos, wp_gt_rottf = self.getcurrentposec("/world", "/workpiece_link")
      # publish wp_gt pose
      ps = geometry_msgs.msg.PoseStamped()

      ps.pose.position.x = wp_gt_pos[0]
      ps.pose.position.y = wp_gt_pos[1]
      ps.pose.position.z = wp_gt_pos[2]

      ps.pose.orientation.x = wp_gt_rottf[0]
      ps.pose.orientation.y = wp_gt_rottf[1]
      ps.pose.orientation.z = wp_gt_rottf[2]
      ps.pose.orientation.w = wp_gt_rottf[3]
      ps.header.frame_id = "world"

      self.t1_1_pub_wp_pose_gt.publish(ps)

   def t1_3_cb_pointed_pose_converter(self, data):
      ''' republish tf frame in normal topic as PoseStamped'''

      rospy.logdebug("received pointed_to pose from team")
      # received pointedto pose from team
      # extract pointer pose from tf
      pt_gt_pos, pt_gt_rottf = self.getcurrentposec("/workpiece_link", "/pointed_to")
      # publish wp_gt pose on self.t1_1_pub_workpiece_pose_gt
      ps = geometry_msgs.msg.PoseStamped()

      ps.pose.position.x = pt_gt_pos[0]
      ps.pose.position.y = pt_gt_pos[1]
      ps.pose.position.z = pt_gt_pos[2]

      ps.pose.orientation.x = pt_gt_rottf[0]
      ps.pose.orientation.y = pt_gt_rottf[1]
      ps.pose.orientation.z = pt_gt_rottf[2]
      ps.pose.orientation.w = pt_gt_rottf[3]
      ps.header.frame_id = "workpiece_link"

      self.t1_3_pub_pointer_pose_gt.publish(ps)

   def execute_task_1_1(self):
      taskid = "1.1"

      # test if other task is running
      if self.active_task != "":
         rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
         return

      self.active_task = taskid
      self.active_task_topic.publish(taskid)

      self.reset_simulator()

      # start logging of relevant topics
      own_topics = {'t1_1_gesturerecog_gt':std_msgs.msg.Bool,
               't1_1_trajectory_index':std_msgs.msg.Int32}
      team_topics = {'t1_1_gesturerecog':std_msgs.msg.Bool}

      self.start_logging(own_topics.keys() + team_topics.keys())

      # init simulator side with task specifics
      # init topics
      self.t1_1_pub_gesture_gt = rospy.Publisher('t1_1_gesturerecog_gt', std_msgs.msg.Bool, latch=True)
      self.t1_1_pub_trajindex = rospy.Publisher('t1_1_trajectory_index', std_msgs.msg.Int32, latch=True)

      self.t1_1_pub_gesture_gt.publish(False)
      rospy.sleep(0.2)
      self.t1_1_pub_trajindex.publish(-1)
      rospy.sleep(1)

      # get trajectories
      traj_pathfns0 = self.get_trajectory_fns(traj_dir_pattern="/trajectories_p0/trajectory_*.yaml")
      traj_pathfns1 = self.get_trajectory_fns(traj_dir_pattern="/trajectories_p1/trajectory_*.yaml")

      rospy.logdebug("found {} human trajectory files".format(len(traj_pathfns0) + len(traj_pathfns1)))

      self.init_task_caller(taskid)

      self.check_topics(team_topics)

      # call solve_task action from client's action server
      goal = SolveTaskGoal(taskid)

      rospy.loginfo("asking client to solve task" + taskid)
      self.client.send_goal(goal)

      # pick 10 trajectories from each gt (0/1)
      import random
      traj_pathfns = random.sample(traj_pathfns0, 10) + random.sample(traj_pathfns1, 10)
      # random order
      random.shuffle(traj_pathfns)

      rospy.loginfo("running the following trajectories: \n{}".format(traj_pathfns))

      traj_pathfns = traj_pathfns[:self.t1_1_trajectory_count]

      for i, traj_fn in enumerate(traj_pathfns):
         # publish ground truth on gt topic
         if traj_fn.endswith('_p1.yaml'):
            self.t1_1_pub_gesture_gt.publish(True)
         else:
            self.t1_1_pub_gesture_gt.publish(False)

         rospy.sleep(0.2)
         # publish current trajectory number on index topic
         self.t1_1_pub_trajindex.publish(i)

         rospy.sleep(1)
         # run trajectory
         rospy.loginfo("running human trajectory file '%s'" % traj_fn)
         human_mover(traj_fn)
         rospy.loginfo("finished running human trajectory file")

      # signal end of task
      self.t1_1_pub_trajindex.publish(-2)

      if not self.client.wait_for_result(rospy.Duration(10)):
         rospy.logwarn("time is up. canceling task solving")

      self.cleanup(taskid)

   def execute_task_1_2(self):
      taskid = "1.2"

      # test if other task is running
      if self.active_task != "":
         rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
         return

      self.active_task = taskid
      self.active_task_topic.publish(taskid)

      self.reset_simulator()

      # start logging of relevant topics
      # gt workpiecepose comes from tf
      own_topics = {"t1_2_workpiece_gt": geometry_msgs.msg.PoseStamped}
      team_topics = {"t1_2_workpiece": geometry_msgs.msg.PoseStamped}
      self.start_logging(own_topics.keys() + team_topics.keys())

      self.init_task_caller(taskid)

      self.check_topics(team_topics)

      # call solve_task action from client's action server
      goal = SolveTaskGoal(taskid)

      # set workpiece pose randomly
      self.change_workpiece_pose()

      time_budget = self.task_time_budget_s[taskid]
      rospy.loginfo("timebudget: " + str(time_budget))
      self.t0_pub_timebudget.publish(time_budget)
      time.sleep(1)

      rospy.loginfo("asking client to solve task" + taskid)
      self.client.send_goal(goal)

      # time count down
      while time_budget >= 0:
         time.sleep(self.timestep)
         time_budget -= self.timestep
         self.t0_pub_timebudget.publish(time_budget)

      if not self.client.wait_for_result(rospy.Duration(10)):
         rospy.logwarn("time is up. canceling task solving")

      self.cleanup(taskid)

   def execute_task_1_3(self):
      taskid = "1.3"

      # test if other task is running
      if self.active_task != "":
         rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
         return

      self.active_task = taskid
      self.active_task_topic.publish(taskid)

      # reset scene
      self.reset_simulator()

      # start logging of relevant topics
      # gt pointer comes from tf
      own_topics = {"t1_3_pointedpose_gt": geometry_msgs.msg.PoseStamped}
      team_topics = {"t1_3_pointedpose": geometry_msgs.msg.PoseStamped}
      self.start_logging(own_topics.keys() + team_topics.keys())

      self.init_task_caller(taskid)

      self.check_topics(team_topics)

      traj_pathfns = self.get_trajectory_fns("/trajectories_p1_wait/trajectory_*.yaml")
      rospy.logdebug("found {} human trajectory files".format(len(traj_pathfns)))

      # call solve_task action from client's action server
      goal = SolveTaskGoal(taskid)

      time_budget = self.task_time_budget_s[taskid]
      rospy.loginfo("timebudget: " + str(time_budget))
      self.t0_pub_timebudget.publish(time_budget)
      time.sleep(1)

      rospy.loginfo("asking client to solve task" + taskid)
      self.client.send_goal(goal)

      # move human and workpiece to position
      random.shuffle(traj_pathfns)
      traj_fn = traj_pathfns[0]

      # set workpiece pose (xyrz) randomly (current pose + jitter)
      self.change_workpiece_pose()

      rospy.loginfo("running human trajectory file '%s'" % traj_fn)
      human_mover(traj_fn, reset_world=False)
      rospy.loginfo("finished running human trajectory file")


      while time_budget >= 0:
         time.sleep(self.timestep)
         time_budget -= self.timestep
         self.t0_pub_timebudget.publish(time_budget)

      if not self.client.wait_for_result(rospy.Duration(10)):
         rospy.logwarn("time is up. cancelling task solving")

      self.cleanup(taskid)

   def execute_task_2(self):
      taskid = "2"

      # test if other task is running
      if self.active_task != "":
         rospy.logerr("cannot execute new task. another task is still running: " + taskid)
         return

      # has to be before setting self.active_task, else the sporadic collisions would trigger the statemachine, which would access this (then uninitialized) publisher
      self.t2_topic_target_frame = rospy.Publisher('target_frame', std_msgs.msg.String, latch=True)
      self.t2_topic_target_frame.publish('rivet_home')

      self.active_task = taskid
      self.active_task_topic.publish(taskid)

      self.reset_simulator()
      self.t2_workpiece_collision = False

      # start logging of relevant topics
      # gt pointer comes from tf
      own_topics = {'target_frame': std_msgs.msg.String,
               'rivet_home': std_msgs.msg.String,
               'collision_human_robot': std_msgs.msg.Bool,
               'collision_workpiece_robot': std_msgs.msg.Bool.data,
               't2_holepose_gt': geometry_msgs.msg.PoseStamped,
               't2_tcppose':  geometry_msgs.msg.PoseStamped
               }
      team_topics = {}
      self.start_logging(own_topics.keys() + team_topics.keys())

      self.init_task_caller(taskid)

      self.check_topics(team_topics)

      self.t2_statemachine('init')

      # call solve_task action from client's action server
      goal = SolveTaskGoal(taskid)

      # start human interference thread
      self.t2_run_human_interference = True
      t = threading.Thread(target=self.do_t2_human_interference)
      t.start()

      time_budget = self.task_time_budget_s[taskid]
      rospy.loginfo("timebudget: " + str(time_budget))
      self.t0_pub_timebudget.publish(time_budget)
      time.sleep(1)

      rospy.loginfo("asking client to solve task" + taskid)
      self.client.send_goal(goal)

      # time count down
      while time_budget >= 0:
         time.sleep(self.timestep)
         time_budget -= self.timestep
         self.t0_pub_timebudget.publish(time_budget)

      if not self.client.wait_for_result(rospy.Duration(10)):
         rospy.logwarn("time is up. cancelling task solving")

      # end human interference thread
      self.t2_run_human_interference = False
      rospy.loginfo("waiting for end of human interference thread")
      t.join(60)
      rospy.loginfo("waiting for end of human interference thread. time out")

      self.cleanup(taskid)

   def cb_execute_task(self, req):
      taskid = req.taskid

      if taskid not in self.task_ids:
         rospy.logerr('invalid task ' + taskid)
         return ExecuteTaskResponse()

      rospy.loginfo('executing task ' + taskid)

      if taskid == '1.1':
         self.execute_task_1_1()
      elif taskid == '1.2':
         self.execute_task_1_2()
      elif taskid == '1.3':
         self.execute_task_1_3()
      elif taskid == '2':
         self.execute_task_2()

      rospy.loginfo('finished executing task ' + taskid)
      return ExecuteTaskResponse()

   def test_find_trajectories(self):
      rospy.loginfo("trajectories for task 1.1: \n" + "\n".join(self.get_trajectory_fns()))
      rospy.loginfo("trajectories for task 1.3: \n" + "\n".join(self.get_trajectory_fns("/trajectories_p1_wait/trajectory_*.yaml")))

   def test_workpiece_positioning(self):
      rospy.loginfo("testing workpiece repositioning")
      for i in xrange(10):
         self.reset_simulator()
         self.change_workpiece_pose()
         time.sleep(5)

   def test_run_challenge(self):
      '''function runs through all tasks and triggers task setup, and initialization and execution of team code'''
      rospy.loginfo("testing C1S1T1 runability")

      found = rospy.wait_for_service('/execute_task')
      execute_task_service = rospy.ServiceProxy('/execute_task', ExecuteTask)

      test_task_ids = self.task_ids
      rospy.loginfo("testing the following tasks: " + str(test_task_ids))

      for taskid in test_task_ids:
         execute_task_service(taskid)
      rospy.loginfo('done with task tests')

def test_robot():
   # connect to robot
   # create tf broadcaster
   # tf_broadcaster.sendTransform(*self.tf_frames_dict[frame_name], time=rospy.Time.now(), child=frame_name, parent="/ur_base")
   pass

def run_tests():
   # test_robot()
   server = com_server()
   server.test_run_challenge()
   # server.test_find_trajectories()
   # server.test_workpiece_positioning()

   pass

if __name__ == "__main__":
   global param_run_as_test
   # init node
   rospy.init_node("com_server_node", log_level=rospy.INFO)

   try:
      param_run_as_test = rospy.get_param("/euroc_test_com_server")
      rospy.loginfo("running com_server in test mode")
   except:
      param_run_as_test = False

   if param_run_as_test:
      run_tests()
   else:
      c_server = com_server()
      rospy.spin()
   pass


[AIS]

I have done some trials and it seems that the problem persists, maybe someone else can confirm it.

After the collision, the /load_rivet service seems to wait some time until the workpiece is reset but the /hole_guess position is still "old".

And sometimes it shows a strange behaviour, I don't know if it is related with the transition between rivet_home and hole_guess in /target_frame topic. I have never noticed it before...