planning_scene_sync.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('tidyup_arm_services')
import rospy
import actionlib
import threading
import sys, traceback, math, copy
from tidyup_msgs import msg
from tidyup_msgs import srv
from std_srvs.srv import Empty, EmptyRequest
from arm_navigation_msgs.msg import PlanningScene, SyncPlanningSceneGoal, SyncPlanningSceneAction
from pr2_python import planning_scene_interface


class PlanningSceneSync():
  def __init__(self):
    self._lock = threading.Lock()
    self._latest_planning_scene = PlanningScene()
    subscribe_service_name = rospy.resolve_name('register_planning_scene')
    rospy.wait_for_service(subscribe_service_name)
    self._sync_action_server = actionlib.SimpleActionServer('~sync_planning_scene', SyncPlanningSceneAction, self._planning_scene_sync_execute, False)
    self._sync_action_server.start() 
    self._subscribe_planning_scene_client = rospy.ServiceProxy(subscribe_service_name, Empty)
    self._subscribe_planning_scene_client.call(EmptyRequest())
    rospy.loginfo('planning scene sync initialized.')

  def _planning_scene_sync_execute(self, goal):
    with self._lock:
      rospy.loginfo('planning scene sync.')
      self._latest_planning_scene = goal.planning_scene
      self.print_robot_pose()
      self.print_attached()
      self._sync_action_server.set_succeeded()
      rospy.loginfo('planning scene sync finished.')

  def update_planning_scene(self, psi):
    with self._lock:
      rospy.loginfo('planning scene updated.')
      # TODO: ok by reference or deepcopy instead??
      psi.current_diff.planning_scene_diff = copy.deepcopy(self._latest_planning_scene)

  def print_attached(self):
    for attached in self._latest_planning_scene.attached_collision_objects:
      rospy.loginfo('attached : {0}, op: {1}, link: {2}'.format(attached.object.id, attached.object.operation.operation, attached.link_name[0]))

  def print_robot_pose(self):
    pose = self._latest_planning_scene.robot_state.multi_dof_joint_state.poses[0]
    rospy.loginfo('robot_pose: {0}, {1}'.format(pose.position.x, pose.position.y))