def _get_robot_state(self):
GET_PLANNING_SCENE_NAME = "/get_planning_scene"
rospy.wait_for_service(GET_PLANNING_SCENE_NAME)
robot_state_client = rospy.ServiceProxy(GET_PLANNING_SCENE_NAME,
GetPlanningScene)
robot_state_req = GetPlanningSceneRequest()
robot_state_req.components.components = robot_state_req.components.ROBOT_STATE
robot_state = robot_state_client(robot_state_req).scene.robot_state
return robot_state
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def __init__(self, height):
self.motion_height = height # Height of the motion to the ground
# Create trajectory server
self.exploration_server = SimpleActionServer('assessment_server',
ExecuteAssesstAction,
self.exploreCallback,
False)
self.server_feedback = ExecuteAssesstFeedback()
self.server_result = ExecuteAssesstResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
#Planning scene client
self.frontiers_client = rospy.ServiceProxy('frontiers_server/find',
Frontiers)
self.frontiers_client.wait_for_service()
self.frontiers_req = FrontiersRequest() #Frontiers request message
# Variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
self.scene_req = GetPlanningSceneRequest()
# Start trajectory server
self.exploration_server.start()
def get_robot_state():
"""
Get current robot state. It calls get_planning_scene service that is
capability of move_group that needs to be enabled.
@returns: Full RobotState
"""
req = GetPlanningSceneRequest()
req.components.components = PlanningSceneComponents.ROBOT_STATE
proxy = rospy.ServiceProxy('get_planning_scene', GetPlanningScene)
try:
res = proxy(req)
except rospy.ServiceException, e:
rospy.logerr("Service did not process request: %s" % str(e))
return None
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo('waiting for object ' + object_name +
' to appear in planning scene...')
get_planning_scene_request = GetPlanningSceneRequest()
get_planning_scene_request.components.components = get_planning_scene_request.components.WORLD_OBJECT_NAMES
# loop until our object appears on scene
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
get_planning_scene_response = self.scene_srv.call(
get_planning_scene_request)
for collision_object in get_planning_scene_response.scene.world.collision_objects:
if collision_object.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo(object_name + ' is in the scene!')
def compute_ik(self, pose):
if isinstance(pose, rox.Transform):
pose = rox_msg.transform2pose_stamped_msg(pose)
assert isinstance(pose, PoseStamped)
if pose.header.frame_id is None or pose.header.frame_id == "":
pose.header.frame_id = "world"
get_planning_scene_req = GetPlanningSceneRequest()
get_planning_scene_req.components.components = PlanningSceneComponents.ROBOT_STATE
get_planning_scene_res = self._get_planning_scene(
get_planning_scene_req)
robot_state_start = get_planning_scene_res.scene.robot_state
ik_request = PositionIKRequest()
ik_request.group_name = self.moveit_group.get_name()
ik_request.robot_state = robot_state_start
ik_request.avoid_collisions = True
ik_request.timeout = rospy.Duration(30.0)
ik_request.pose_stamped = pose
ik_request.ik_link_name = self.moveit_group.get_end_effector_link()
for i in xrange(len(robot_state_start.joint_state.name)):
j = JointConstraint()
j.joint_name = robot_state_start.joint_state.name[i]
j.position = robot_state_start.joint_state.position[i]
j.tolerance_above = np.deg2rad(170)
j.tolerance_below = np.deg2rad(170)
j.weight = 100
ik_request.constraints.joint_constraints.append(j)
ik_request_req = GetPositionIKRequest(ik_request)
ik_request_res = self._compute_ik(ik_request_req)
if (ik_request_res.error_code.val != MoveItErrorCodes.SUCCESS):
raise Exception("Could not compute inverse kinematics")
return ik_request_res.solution.joint_state.position
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo(
"Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
#HEARTS tried inputting multiple cubes to approximate cylinder
#TODO re-investigate if time allows
#def rotate_quat(self,quat,angle_deg,axis_euler):
'''
