def __init__(self):
self.allowed_planning_time = 10.0
self.fixed_frame = 'base_link'
self.gripper_frame = 'wrist_roll_link'
self.group_name = 'arm'
self.planning_scene_diff = moveit_msgs.msg.PlanningScene()
self.planning_scene_diff.is_diff = True
self.planning_scene_diff.robot_state.is_diff = True
self.look_around = False
self.max_acceleration_scaling_factor = 0
self.max_velocity_scaling_factor = 0
self.num_planning_attempts = 1
self.plan_only = False
self.planner_id = 'RRTConnectkConfigDefault'
self.replan = False
self.replan_attempts = 5
self.replan_delay = 1
self.start_state = moveit_msgs.msg.RobotState()
self.start_state.is_diff = True
self.tolerance = 0.01
self._orientation_contraints = []
self._pose_goal = None
self._joint_names = None
self._joint_positions = None
self._tf_listener = TransformListener()
def __init__(self, name, server, menu_handler, frame_world,
frame_opt_parent, frame_opt_child, frame_sensor,
marker_scale):
print('Creating a new sensor named ' + name)
self.name = name
self.server = server
self.menu_handler = menu_handler
self.listener = TransformListener()
self.br = tf.TransformBroadcaster()
self.marker_scale = marker_scale
# transforms
self.world_link = frame_world
self.opt_parent_link = frame_opt_parent
self.opt_child_link = frame_opt_child
self.sensor_link = frame_sensor
self.updateAll() # update all the transformations
# print('Collected pre, opt and pos transforms.')
#
# print('preT:\n' + str(self.preT))
# print('optT:\n' + str(self.optT))
# print('posT:\n' + str(self.posT))
self.optTInitial = copy.deepcopy(self.optT)
self.createInteractiveMarker() # create interactive marker
print('Created interactive marker.')
# Start publishing now
self.timer_callback = rospy.Timer(
rospy.Duration(.1),
self.publishTFCallback) # to periodically broadcast
def __init__(self,
group,
fixed_frame,
gripper_frame,
cart_srv,
listener=None,
plan_only=False):
self._group = group
self._fixed_frame = fixed_frame
self._gripper_frame = gripper_frame # a.k.a end-effector frame
self._action = actionlib.SimpleActionClient('move_group',
MoveGroupAction)
self._traj_action = actionlib.SimpleActionClient(
'execute_trajectory', ExecuteTrajectoryAction)
self._cart_service = rospy.ServiceProxy(cart_srv, GetCartesianPath)
try:
self._cart_service.wait_for_service(timeout=3)
except:
rospy.logerr("Timeout waiting for Cartesian Planning Service!!")
self._action.wait_for_server()
if listener == None:
self._listener = TransformListener()
else:
self._listener = listener
self.plan_only = plan_only
self.planner_id = None
self.planning_time = 15.0
def __init__(self):
# enables
self.enable_tilt = True
self.enable_look = True
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
'head_controller/point_head', PointHeadAction)
self.client.wait_for_server()
self.service = rospy.Service('~configure', TiltControl,
self.serviceHandler)
self.plan_sub = rospy.Subscriber(
'move_base/TrajectoryPlannerROS/local_plan', Path,
self.planCallback)
self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray,
self.statCallback)
def __init__(self):
self.pose = PoseStamped()
self._tf_listener = TransformListener()
#self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10)
#self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback)
self.joint_srv = rospy.Service('tool_pose', GetToolPose,
self.tool_pose_callback)
def __init__(self, name):
# wait for moveit
while not "/move_group/result" in dict(
rospy.get_published_topics()).keys():
rospy.sleep(2)
self.group = MoveGroupCommander("arm")
self.group.set_start_state_to_current_state()
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_pose_reference_frame("/base_footprint")
self.group.set_max_velocity_scaling_factor(1)
self.group.set_num_planning_attempts(50)
self.group.set_planning_time(10)
self.group.set_goal_position_tolerance(0.01)
self.group.set_goal_orientation_tolerance(0.02)
self.tf_listener = TransformListener()
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
elevator.msg.SimpleTargetAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def __init__(self, args):
rospy.init_node('roadmap_server')
self.optimization_distance = rospy.get_param('~optimization_distance', 10);
self.tf = TransformListener()
stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
self.skel = Skeleton(stereo_cam)
# self.skel.node_vdist = 1
if False:
self.skel.load(args[1])
self.skel.optimize()
self.startframe = 100000
else:
self.startframe = 0
self.frame_timestamps = {}
self.vo = None
self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap)
time.sleep(1)
#self.send_map(rospy.time(0))
self.wheel_odom_edges = set()
rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
def get_cube_names(self, pose):
point = PoseStamped()
point.header.frame_id = 'base_link'
point.header.stamp = rospy.Time()
point.pose = pose
tf_listener = TransformListener()
rospy.sleep(1.0)
return tf_listener.transformPose('/map', point)
def __init__(self):
self.manip = mic.MoveGroupCommander("manipulator")
self.client = act.SimpleActionClient('joint_trajectory_action',
trajAction)
rp.loginfo("Waiting for server joint_trajectory_action.")
self.client.wait_for_server()
rp.loginfo("Successfuly connected to server.")
self.tfListener = TransformListener()
def __init__(self):
self._joint_client = actionlib.SimpleActionClient(
JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction)
self._joint_client.wait_for_server(rospy.Duration(10))
self._move_group_client = actionlib.SimpleActionClient(
MOVE_GROUP_ACTION_SERVER, MoveGroupAction)
self._move_group_client.wait_for_server(rospy.Duration(10))
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._tf_listener = TransformListener()
def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature]
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
filtered = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
if point.point.x <= max_distance:
filtered.append(feature)
return filtered
def __init__(self):
self.moving = False
self.listener = TransformListener()
self.broadcaster = TransformBroadcaster()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback)
self.lastFaceCallback = time.time()
self.lastHeadTarget = None
def __init__(self, group, frame, listener=None, plan_only=False):
self._group = group
self._fixed_frame = frame
self._action = actionlib.SimpleActionClient('move_group',
MoveGroupAction)
self._action.wait_for_server()
if listener == None:
self._listener = TransformListener()
else:
self._listener = listener
self.plan_only = plan_only
self.planner_id = None
self.planning_time = 15.0
def feature_depths(features):
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
depths = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()),
rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z]))
return depths
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
self.move_group = MoveGroupInterface("left_arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server(rospy.Duration(15.0))
self.cubes = []
self.tf_listener = TransformListener()
# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
# self.gripper_client.wait_for_server()
# rospy.loginfo("...connected.")
rospy.sleep(2.0)
def execute(self, userdata):
rospy.logdebug('CamToDropzone: Executing state CamToDropzone')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
utils.manipulation.set_turbo_mode()
utils.manipulation.set_movement_time(9)
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("drop_point"):
if not (now - then < abort_after):
rospy.logdebug('drop_point frame not found!')
return 'fail'
rospy.loginfo("wait for drop_point frame")
rospy.sleep(2.)
now = int(time.time())
then = int(time.time())
now = int(time.time())
while not listener.frameExists("mdl_middle"):
if not (now - then < abort_after):
rospy.logdebug('CamToDropzone: mdl_middle frame not found!')
return 'fail'
rospy.sleep(2.)
now = int(time.time())
if len(userdata.scan_conveyor_pose) == 0:
rospy.logdebug('CamToDropzone: No scanPose, calculate now...')
p = utils.manipulation.scan_conveyor_pose()
userdata.scan_conveyor_pose.append(p)
else:
rospy.logdebug('CamToDropzone: scanPose found!')
p = userdata.scan_conveyor_pose[0]
rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose')
for i in range(0, 3):
if utils.manipulation.move_to(p):
return 'scanPoseReached'
else:
if i == 2:
rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose')
return 'fail'
def select_center(features): # type: (List[Feature]) -> Feature
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
angles = []
center_ray = np.array([1.0, 0.0, 0.0])
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
ray = np.array([point.point.x, point.point.y, point.point.z])
ray /= np.linalg.norm(ray)
angle = np.arccos(np.dot(center_ray, ray))
angles.append(abs(angle))
return features[np.argmin(angles)]
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_objects",
PerceiveButtons,
self.do_perception)
self.action_srv = rospy.Service("/clickable_world/grasp_object",
ButtonAction,
self.do_action)
self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect",
ObjectButtonDetect)
self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction)
self.grasp_setup_client.wait_for_server()
def transform_pose(_pose2D, src_frame='CameraTop_frame', dst_frame='/base_footprint', timeout=3):
tl = TransformListener()
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = Pose(Point(_pose2D.x, _pose2D.y, 0.0), Quaternion())
try:
tl.waitForTransform(target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = tl.transformPose(dst_frame, pose_stamped)
except Exception as e:
rospy.logwarn("Transformation failed!!! %s" % str(e))
return _pose2D
return Pose2D(pose_transf.pose.position.x, pose_transf.pose.position.y, 0.0)
def __init__(self):
self.last_vel_time = rospy.Time(0)
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path,
self.planCallback)
self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist,
self.velCallback)
def __init__(self):
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber(
"move_base/TrajectoryPlannerROS/local_plan", Path,
self.planCallback)
self.stat_sub = rospy.Subscriber("move_base/status", GoalStatusArray,
self.statCallback)
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service(
"/clickable_world/place_table_perceive", PerceiveButtons,
self.do_perception)
self.action_srv = rospy.Service("/clickable_world/place_object",
ButtonAction, self.do_action)
self.pc_capture_srv = rospy.ServiceProxy(
"/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty)
self.table_seg_srv = rospy.ServiceProxy(
"/table_detection/segment_surfaces", SegmentSurfaces)
self.grasp_client = actionlib.SimpleActionClient(
self.arm + '_overhead_grasp', OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(
self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction)
self.grasp_setup_client.wait_for_server()
def __init__(self, *args):
TransformListener.__init__(self)
python_actions.Action.__init__(self, args[0], args[1], args[2],
args[3])
self.name = args[0]
try:
self.head_controller = rospy.get_param(self.name +
"/head_controller")
except KeyError:
self.head_controller = "head_controller"
rospy.set_param(self.name + "/head_controller",
self.head_controller)
self.head_controller_publisher = rospy.Publisher(
self.head_controller + "/command", mechanism_msgs.msg.JointStates)
self.people_sub = rospy.Subscriber(
"/face_detector/people_tracker_measurements", PositionMeasurement,
self.people_position_measurement)
self.face_det = rospy.ServiceProxy('/start_detection', StartDetection)
def transform_pose(pose, src_frame, dst_frame, timeout=10):
"""
Transforms the given pose from src_frame to dst_frame.
:param src_frame
:param dst_frame
:param timeout the amount of time allowed (in secs) for a transformation (default 3)
"""
if str(type(pose)) != str(type(Pose())):
rospy.logwarn(colors.BACKGROUND_RED + "The 'pose' should be a Pose object, not '%s'.%s" % (str(type(pose)).split('\'')[1], colors.NATIVE_COLOR))
from tf.listener import TransformListener
assert timeout >= 1
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = pose
global _tl
if not _tl:
_tl = TransformListener()
rospy.sleep(0.5)
timeout -= 0.5
rospy.loginfo("Transforming position from %s to %s coordinates..." % (
src_frame, dst_frame))
# If something fails we'll return the original pose (for testing
# with mocks when tf isn't available)
result = pose
try:
_tl.waitForTransform(
target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = _tl.transformPose(dst_frame, pose_stamped)
result = pose_transf.pose
except Exception as e:
rospy.logwarn(colors.BACKGROUND_RED + "Warning! Pose transformation failed! %s%s" % (str(e), colors.NATIVE_COLOR))
return result
class MoveFloorButton:
def __init__(self):
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor",
PerceiveButtons,
self.do_perception)
self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis",
Marker)
self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped)
self.action_srv = rospy.Service("/clickable_world/move_empty_floor",
ButtonAction,
self.do_action)
self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect",
SegmentFloor)
self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("[move_floor_button] MoveFloorButton loaded.")
def do_perception(self, req):
# segment surfaces
rospy.loginfo("[move_floor_button] Segmenting floor")
self.surfaces = self.floor_seg_srv().surfaces
for i in range(len(self.surfaces)):
self.surfaces[i].color.r = 0
self.surfaces[i].color.g = 0
self.surfaces[i].color.b = 256
self.surfaces[i].color.a = 256
if len(self.surfaces) != 0:
self.percept_pub.publish(self.surfaces[0])
resp = PerceiveButtonsResponse()
resp.buttons = self.surfaces
return resp
def do_action(self, req):
rospy.loginfo("[move_floor_button] MoveFloorButton clicked!")
# put 3d pt in base frame
req.pixel3d.header.stamp = rospy.Time(0)
move_pt = self.tl.transformPoint("/base_link", req.pixel3d)
floor_move_goal = MoveBaseGoal()
floor_move_goal.target_pose.header.frame_id = move_pt.header.frame_id
floor_move_goal.target_pose.pose.position = move_pt.point
quat = tf_trans.quaternion_from_euler(0, 0, np.arctan2(move_pt.point.y, move_pt.point.x))
floor_move_goal.target_pose.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
floor_move_goal.target_pose.header.frame_id = "/base_link"
floor_move_goal.target_pose.header.stamp = rospy.Time.now()
try:
self.goal_pub.publish(floor_move_goal.target_pose)
self.floor_move_client.send_goal(floor_move_goal)
self.floor_move_client.wait_for_result()
cur_pose = self.floor_move_client.get_result()
except rospy.ROSInterruptException:
print "[move_floor_button] Move floor failed"
return ButtonActionResponse()
def __init__(self):
# Internal parameters
self.publish_rate = rospy.get_param("~publish_rate", 10.0)
self.listener = TransformListener()
self.desired_pose_frame = rospy.get_param("~desired_pose_frame",
"base_link")
self.position_match_threshold = rospy.get_param(
"~position_match_threshold", 1.0)
# Variables to keep track of state
self.closest_person = None
self.leg_detections = []
self.closest_person_lock = Lock()
self.leg_detections_lock = Lock()
# Internal helpers
self.person_face_distance_func = lambda A, B: np.sqrt(
(A.pose.position.x - B.pos.x)**2 +
(A.pose.position.y - B.pos.y)**2 +
(1.2 - B.pos.z)**2 # A person's face is about 4ft from the floor
)
self.leg_detection_is_closest_face = lambda detected_person: (
self.closest_person is not None and self.closest_person.
detection_context.pose_source == DetectionContext.POSE_FROM_FACE
and self.closest_person.id == detected_person.id)
# The subscribers and publishers
self.face_sub = rospy.Subscriber(
"face_detector/people_tracker_measurements_array",
PositionMeasurementArray, self.face_callback)
self.leg_sub = rospy.Subscriber("people_tracked", PersonArray,
self.leg_callback)
self.closest_person_pub = rospy.Publisher('~closest_person',
Person,
queue_size=10)
# Debug
self.debug_enabled = rospy.get_param("~debug", False)
if self.debug_enabled:
self.debug_pub1 = rospy.Publisher("~debug/1", Marker, queue_size=1)
self.debug_pub2 = rospy.Publisher("~debug/2", Marker, queue_size=1)
def execute(self, userdata):
rospy.logdebug('PlaceTask6: Executing state PlaceTask6')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("target_zone"):
if not now - then < abort_after:
rospy.logdebug("PlaceTask6: target_zone frame not found")
return 'fail'
rospy.loginfo("wait for target_zone frame")
rospy.sleep(2.)
now = int(time.time())
self.__place_pose = self.get_place_point(userdata)
rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
rospy.logdebug(self.__place_pose)
for i in range(0, 7):
if utils.manipulation.move_to(self.__place_pose):
rospy.logdebug('PlaceTask6: OpenGripper')
utils.manipulation.open_gripper()
return 'success'
else:
rospy.logdebug('PlaceTask6: Calculate new place position...')
if i <= 2:
self.__place_pose = self.get_place_point(userdata)
if i > 2:
self.__place_pose = self.get_place_point(userdata, 3)
if i == 6:
rospy.logdebug('PlaceTask6: Cant find place position!')
utils.manipulation.open_gripper()
return 'fail'
def execute(self, userdata):
rospy.logdebug('PlaceTask6: Executing state PlaceTask6')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("target_zone"):
if not now - then < abort_after:
rospy.logdebug("PlaceTask6: target_zone frame not found")
return 'fail'
rospy.loginfo("wait for target_zone frame")
rospy.sleep(2.)
now = int(time.time())
self.__place_pose = self.get_place_point(userdata)
rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
rospy.logdebug(self.__place_pose)
for i in range(0, 7):
if utils.manipulation.move_to(self.__place_pose):
rospy.logdebug('PlaceTask6: OpenGripper')
utils.manipulation.open_gripper()
return 'success'
else:
rospy.logdebug('PlaceTask6: Calculate new place position...')
if i <= 2:
self.__place_pose = self.get_place_point(userdata)
if i > 2:
self.__place_pose = self.get_place_point(userdata, 3)
if i == 6:
rospy.logdebug('PlaceTask6: Cant find place position!')
utils.manipulation.open_gripper()
return 'fail'
class ToolPose(object):
def __init__(self):
self.pose = PoseStamped()
self._tf_listener = TransformListener()
#self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10)
#self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback)
self.joint_srv = rospy.Service('tool_pose', GetToolPose,
self.tool_pose_callback)
def tool_pose_callback(self, req):
''' get gripper link pose with respect to base_link'''
res = GetToolPoseResponse()
time = 0
trans = None
rot = None
self.pose.header.frame_id = 'base_link'
while not rospy.is_shutdown():
try:
time = self._tf_listener.getLatestCommonTime(
'base_link', 'wrist_roll_link')
trans, rot = self._tf_listener.lookupTransform(
'base_link', 'wrist_roll_link', time)
break
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
continue
self.pose.pose.position.x = round(trans[0], 4)
self.pose.pose.position.y = round(trans[1], 4)
self.pose.pose.position.z = round(trans[2], 4)
self.pose.pose.orientation.x = round(rot[0], 4)
self.pose.pose.orientation.y = round(rot[1], 4)
self.pose.pose.orientation.z = round(rot[2], 4)
self.pose.pose.orientation.w = round(rot[3], 4)
#self.pose_pub.publish(self.pose)
res.tool_pose = self.pose
return res
def main():
rospy.init_node('find_waypoints')
joy_subscriber = SubscriberValue('joy', Joy)
tf_listener = TransformListener()
names = [
'initial_pose', 'off_ramp_start', 'off_ramp_end', 'ar_search_1',
'ar_search_2', 'S1', 'S2', 'S3', 'S4', 'S5', 'S6', 'S7', 'S8',
'on_ramp'
]
coords = []
for name in names:
rospy.sleep(rospy.Duration(2))
print(name)
while not joy_subscriber.value.buttons[0]:
rospy.sleep(rospy.Duration(0, 1000))
pose = tf_listener.lookupTransform('/map', '/base_link', rospy.Time())
coords.append(pose)
print('Saved')
out_file = open(
path.join(path.dirname(__file__), '..', 'param',
'find_waypoints_generated.yaml'), 'w')
out_file.write('named_poses:\n')
for name, ((x, y, z), (rx, ry, rz, rw)) in zip(names, coords):
out_file.write(' {name}:\n'.format(name=name))
out_file.write(' position:\n')
out_file.write(' - {x}\n'.format(x=x))
out_file.write(' - {y}\n'.format(y=y))
out_file.write(' - {z}\n'.format(z=z))
out_file.write(' orientation:\n')
out_file.write(' - {x}\n'.format(x=rx))
out_file.write(' - {y}\n'.format(y=ry))
out_file.write(' - {z}\n'.format(z=rz))
out_file.write(' - {w}\n'.format(w=rw))
def __init__(self):
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor",
PerceiveButtons,
self.do_perception)
self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis",
Marker)
self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped)
self.action_srv = rospy.Service("/clickable_world/move_empty_floor",
ButtonAction,
self.do_action)
self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect",
SegmentFloor)
self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("[move_floor_button] MoveFloorButton loaded.")
def __init__(self):
self.last_vel_time = rospy.Time(0)
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback)
self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback)
def __init__(self):
self.marker_pub = rospy.Publisher(HUMAN_TRIGGER_MARKER_PUB,
Marker,
queue_size=10)
self.coord_signal = rospy.Publisher(COORD_SIGNAL_PUB,
CoordinationSignal,
queue_size=1)
self.tf_listener = TransformListener()
self.move_base = SimpleActionClient(MOVE_BASE_ACTION_SRV,
MoveBaseAction)
rospy.loginfo(
"{} waiting for move_base action server.".format(NODE_NAME))
self.move_base.wait_for_server()
rospy.loginfo("{} found move_base action server.".format(NODE_NAME))
self.navigating_fact_id = None
rospy.loginfo(NODE_NAME +
" waiting for underworlds start fact service server.")
self.start_fact = rospy.ServiceProxy(START_FACT_SRV_NAME, StartFact)
self.start_fact.wait_for_service()
rospy.loginfo(NODE_NAME +
" found underworlds start fact service server.")
rospy.loginfo(NODE_NAME +
" waiting for underworlds stop fact service server.")
self.end_fact = rospy.ServiceProxy(STOP_FACT_SRV_NAME, EndFact)
self.end_fact.wait_for_service()
rospy.loginfo(NODE_NAME +
" found underworlds stop fact service server.")
self.coordination_human_timer = None
self.trigger_zone_bb = BoundingBox(3.0, 13.0, 5.0, 16.0)
self.state = self.State.IDLE
rospy.Timer(rospy.Duration(1.0),
lambda _: self.draw_trigger_zone(False),
oneshot=True)
rospy.Timer(rospy.Duration(0.1), self.loop)
def jointsInfo(printoutRate=0.5, anonym=False):
"""
Creates new node to listen to /joint_states topic\n
Usage:\n
- printoutRate - print rate to terminal in Hz
- default: 0.5 Hz
- anonym [True/False] - do you want to run it as anonymous node
- default: False
"""
rp.init_node("abb_jointListener", anonymous=anonym)
tfListener = TransformListener()
listener = rp.Subscriber("/joint_states", JointState, __listenCb,
tfListener)
rate = rp.Rate(printoutRate)
rate.sleep()
rp.spin()
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service(
"/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception
)
self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action)
self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty)
self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces)
self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(
self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction
)
self.grasp_setup_client.wait_for_server()
def __init__(self):
# enables
self.enable_tilt = True
self.enable_look = True
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.client.wait_for_server()
self.service = rospy.Service('~configure', TiltControl, self.serviceHandler)
self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback)
self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback)
class Transformer:
def __init__(self):
self.__listener = TransformListener()
def __del__(self):
pass
def transform_to(self, pose_target, target_frame="/odom_combined"):
'''
Transforms the pose_target into the target_frame.
:param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2
:param target_frame: goal frame id
:return: transformed object
'''
if pose_target.header.frame_id == target_frame:
return pose_target
odom_pose = None
i = 0
while odom_pose is None and i < 10:
try:
#now = rospy.Time.now()
#self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4))
if type(pose_target) is CollisionObject:
i = 0
new_co = deepcopy(pose_target)
for cop in pose_target.primitive_poses:
p = PoseStamped()
p.header = pose_target.header
p.pose.orientation = cop.orientation
p.pose.position = cop.position
p = self.transform_to(p, target_frame)
if p is None:
return None
new_co.primitive_poses[i].position = p.pose.position
new_co.primitive_poses[i].orientation = p.pose.orientation
i += 1
new_co.header.frame_id = target_frame
return new_co
if type(pose_target) is PoseStamped:
odom_pose = self.__listener.transformPose(target_frame, pose_target)
break
if type(pose_target) is Vector3Stamped:
odom_pose = self.__listener.transformVector3(target_frame, pose_target)
break
if type(pose_target) is PointStamped:
odom_pose = self.__listener.transformPoint(target_frame, pose_target)
break
if type(pose_target) is PointCloud2:
odom_pose = self.__listener.transformPointCloud(target_frame, pose_target)
break
except Exception, e:
print "tf error:::", e
rospy.sleep(0.5)
i += 1
rospy.logdebug("tf fail nr. " + str(i))
if odom_pose is None:
rospy.logerr("FUUUUUUUUUUUUUU!!!! f*****g tf shit!!!!")
return odom_pose
class NavHeadController:
def __init__(self):
# enables
self.enable_tilt = True
self.enable_look = True
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.client.wait_for_server()
self.service = rospy.Service('~configure', TiltControl, self.serviceHandler)
self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback)
self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback)
def serviceHandler(self, req):
self.enable_tilt = req.enable_tilt
self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look
return TiltControlResponse()
def statCallback(self, msg):
goal = False
for status in msg.status_list:
if status.status == GoalStatus.ACTIVE:
goal = True
break
self.has_goal = goal
def planCallback(self, msg):
# Only need to do this if we are looking
if not self.enable_look:
return
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
point = [1, 0, 0, 1]
M = R*point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0,0]
p.point.y += pose_stamped.pose.position.y + M[1,0]
p.point.z += pose_stamped.pose.position.z + M[2,0]
# transform to base_link
p = self.listener.transformPoint('base_link', p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if self.has_goal and self.enable_tilt:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = 'base_link'
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
def __init__(self):
self.__listener = TransformListener()
class SEMAPEnvironmentServices():
server = None
objects = {}
tf_broadcaster = None
tf_listener = None
marker_pub = None
range_query2d_marker = None
range_query3d_marker = None
range_query_marker = None
area_query_marker = None
volume_query_marker = None
def __init__(self):
self.setup_node()
self.server = InteractiveMarkerServer("semap_environment")
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
def setup_node(self):
rospy.init_node('semap_environment_services')
rospy.loginfo( "SEMAP Environment Services are initializing...\n" )
rospy.Timer(rospy.Duration(0.02), self.publishTF)
#rospy.Timer(rospy.Duration(1.0), self.activate_robot_surroundings)
rospy.Subscriber("create_object", PoseStamped, self.createObjectCb)
#rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinPolygonCb)
#rospy.Subscriber("polygon", PolygonStamped, self.copyObjectWithinPolygonCb)
rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinVolumeCb)
rospy.Subscriber("range2d", PointStamped, self.findObjectWithinRange2DCb)
rospy.Subscriber("range_query", PointStamped, self.findObjectWithinRangeCb)
self.range_query_marker = rospy.Publisher("distance_query", MarkerArray, queue_size=10)
self.area_query_marker = rospy.Publisher("area_query", PolygonStamped, queue_size=10)
self.volume_query_marker = rospy.Publisher("volume_query", MarkerArray, queue_size=10)
self.range_query2d_marker = rospy.Publisher("range_query2d_marker", Marker, queue_size=10)
self.marker_pub = rospy.Publisher("collection_marker", MarkerArray, queue_size=10)
user = rospy.get_param('~user')
password = rospy.get_param('~password')
host = rospy.get_param('~host')
database = rospy.get_param('~database')
initializeConnection(user, password, host, database, False)
## Active Objects
srv_refresh_objects = rospy.Service('refresh_objects', ActivateObjects, self.refresh_objects)
srv_refresh_all_objects = rospy.Service('refresh_all_objects', ActivateAllObjects, self.refresh_all_objects)
srv_activate_objects = rospy.Service('activate_objects', ActivateObjects, self.activate_objects)
srv_activate_all_objects = rospy.Service('activate_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_deactivate_objects = rospy.Service('deactivate_objects', DeactivateObjects, self.deactivate_objects)
srv_deactivate_all_object = rospy.Service('deactivate_all_objects', DeactivateAllObjects, self.deactivate_all_objects)
srv_show_objects = rospy.Service('show_objects', ActivateObjects, self.show_objects)
srv_show_all_objects = rospy.Service('show_all_objects', ActivateAllObjects, self.show_all_objects)
#srv_unshow_objects = rospy.Service('unshow_objects', ActivateObjects, self.activate_all_objects)
#srv_unshow_all_objects = rospy.Service('unshow_all_objects', ActivateAllObjects, self.activate_all_objects)
srv_show_distance_between_objects = rospy.Service('show_distance_between_objects', GetDistanceBetweenObjects, self.showDistanceBetweenObjects)
srv_show_objects_within_range = rospy.Service('show_objects_within_range', GetObjectsWithinRange, self.showObjectWithinRange)
srv_show_objects_within_area = rospy.Service('show_objects_within_area', GetObjectsWithinArea, self.showObjectWithinArea)
srv_show_objects_within_volume = rospy.Service('show_objects_within_volume', GetObjectsWithinVolume, self.showObjectWithinVolume)
srv_activate_objects_in_objects = rospy.Service('activate_objects_in_objects', GetObjectsInObjects, self.activateObjectsInObjects)
srv_show_objects_in_objects = rospy.Service('show_objects_in_objects', GetObjectsInObjects, self.showObjectsInObjects)
srv_activate_objects_on_objects = rospy.Service('activate_objects_on_objects', GetObjectsOnObjects, self.activateObjectsOnObjects)
srv_show_objects_on_objects = rospy.Service('show_objects_on_objects', GetObjectsOnObjects, self.showObjectsOnObjects)
rospy.loginfo( "SEMAP DB Services are running...\n" )
def spin(self):
rospy.spin()
### Services
def activate_objects(self, req):
then = rospy.Time.now()
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: activate_objects")
#self.deactivate_objects(req)
get_res = call_get_object_instances(req.ids)
rospy.loginfo("Call Get Object Instances took %f seconds" % (rospy.Time.now() - then).to_sec())
for obj in get_res.objects:
#rospy.loginfo("Activate object: %s" % obj.name)
# get inst visu from db
# if exits: convertNonDBtype
# else create one, store and pass on
if obj.name in self.objects.keys():
#if self.objects[obj.name].status == "active":
#print 'object', obj.name, 'is already active and gets reactivated'
if self.objects[obj.name].status == "inactive":
del self.objects[obj.name]
#print 'object', obj.name, 'is inactive and gets removed from inactive pool'
#else:
# print 'object was unknown so far, now its active'
then2 = rospy.Time.now()
active_object = EnvironmentObject(obj.id, obj.name, obj, InteractiveObjectMarker(obj, self.server, None ), status = "active")
#rospy.loginfo("Marker took %f seconds" % (rospy.Time.now() - then2).to_sec())
self.objects[obj.name] = active_object
#rospy.loginfo("Took %f seconds" % (rospy.Time.now() - now).to_sec())
self.publishTF(None)
rospy.loginfo("Took %f seconds" % (rospy.Time.now() - then).to_sec())
rospy.loginfo("SpatialEnv SRVs: activate_objects - done")
return ActivateObjectsResponse()
def show_objects(self, req):
if len( req.ids) > 0:
rospy.loginfo("SpatialEnv SRVs: show_objects")
get_res = call_get_object_instances(req.ids)
for obj in get_res.objects:
if obj.name in self.objects.keys():
if self.objects[obj.name].status == "active":
print 'object', obj.name, 'is already active'
break
elif self.objects[obj.name].status == "inactive":
print 'object', obj.name, 'is inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
else:
print 'object was unknown so far, now its inactive'
ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive")
self.objects[obj.name] = ghost
rospy.loginfo("SpatialEnv SRVs: show_objects - done")
return ActivateObjectsResponse()
def refresh_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_objects")
print req.ids
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_objects - done")
return res
def refresh_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects")
active_req = ActivateObjectsRequest()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].status == "active":
active_req.ids.append( self.objects[key].id )
elif self.objects[key].status == "inactive":
inactive_req.ids.append( self.objects[key].id )
self.activate_objects(active_req)
self.show_objects(inactive_req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects - done")
return res
def deactivate_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_objects")
res = DeactivateObjectsResponse()
inactive_req = ActivateObjectsRequest()
for key in self.objects.keys():
if self.objects[key].id in req.ids:
del self.objects[key]
self.show_objects(req)
self.publishTF(None)
return res
def deactivate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: deactivate_all_objects")
self.objects = {}
self.publishTF(None)
self.show_all_objects(req)
return DeactivateAllObjectsResponse()
def activate_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: activate_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.activate_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: activate_all_objects - done")
return res
def show_all_objects(self, req):
rospy.loginfo("SpatialEnv SRVs: show_all_objects")
ids = db().query(ObjectInstance.id).all()
req = ActivateObjectsRequest()
req.ids = [id for id, in ids]
self.show_objects(req)
res = ActivateAllObjectsResponse()
rospy.loginfo("SpatialEnv SRVs: show_all_objects - done")
return res
def publishTF(self, msg):
if len(self.objects) > 0:
for key in self.objects.keys():
if self.objects[key].status == "active":
object = self.objects[key].object
translation = object.pose.pose.position
rotation = object.pose.pose.orientation
time = rospy.Time.now()
#print 'publish tf'
self.tf_broadcaster.sendTransform( (translation.x, translation.y, translation.z), \
(rotation.x, rotation.y, rotation.z, rotation.w), \
time, object.name, object.pose.header.frame_id)
def activate_robot_surroundings(self, msg):
#if activate_robot_surroundings:
try:
(trans,rot) = self.tf_listener.lookupTransform('/world', '/base_link', rospy.Time(0))
print 'current robot position', trans
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'could not determine current robot position'
#else:
# robot inactive
def createObjectCb(self, pose):
print 'Create Object from RViz pose'
app = QApplication(sys.argv)
widget = ChooseObjectDescriptionWidget(0)
widget.show()
app.exec_()
desc_name, desc_id = widget.getChoice()
del app, widget
if(desc_id == -1):
new_desc = ROSObjectDescription()
new_desc.type = desc_name
res = call_add_object_descriptions( [ new_desc ] )
print res
desc_id = res.ids[0]
obj = ROSObjectInstance()
obj.description.id = desc_id
obj.pose = pose
res = call_add_object_instances( [obj] )
call_activate_objects( res.ids )
def activateObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsInObjects(self, req):
res = call_get_objects_in_objects(req.reference, req.target, req.fully_within)
ids = []
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def activateObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
return res
def showObjectsOnObjects(self, req):
print 'GOT SO FAR'
res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold)
print 'NOW WHAT'
ids = []
for pair in res.pairs:
#if pair.reference_id not in ids:
# ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_show_objects( ids )
return res
def findObjectWithinRange2DCb(self, point_stamped):
distance = 2.0
print 'find Objects Within Range2D'
#res = call_get_objects_within_range2d([], "Position2D", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d([], "FootprintBox", point_stamped.point, distance, fully_within = False)
#res = call_get_objects_within_range2d(["ConferenceTable", "ConferenceChair"], "Position2D", point_stamped.point, 3.0)
#call_activate_objects( res.ids )
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "2d_range_query_marker"
marker.id = 0
marker.type = 3 #cylinder
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.01
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.5
self.range_query2d_marker.publish(marker)
def findObjectWithinRangeCb(self, point_stamped):
distance = 1.5
res = call_get_objects_within_range(point_stamped.point, [], "FootprintHull", distance, fully_within = False)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = distance*2
marker.scale.y = distance*2
marker.scale.z = 0.005
marker.pose.position = point_stamped.point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
def showObjectWithinRange(self, req):
res = call_get_objects_within_range(req.reference_point, req.target_object_types, req.target_object_geometry_type,
req.distance, req.fully_within)
ids = []
array = MarkerArray()
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.get_rostime()
marker.ns = "range"
marker.id = 0
marker.type = 3 #sphere
marker.scale.x = req.distance*2
marker.scale.y = req.distance*2
marker.scale.z = 0.005
marker.pose.position = req.reference_point
marker.pose.position.z = 0.0
marker.color.r = 0.25
marker.color.g = 0.25
marker.color.b = 0.75
marker.color.a = 0.5
array.markers.append(marker)
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def findObjectWithinVolumeCb(self, polygon_stamped):
extrude = call_extrude_polygon(polygon_stamped.polygon, 0.0, 0.0, 0.9)
mesh = PolygonMeshStamped()
mesh.header.frame_id = "world"
mesh.mesh = extrude.mesh
print extrude.mesh
res = call_get_objects_within_volume(extrude.mesh, ['Mug','Teapot','Monitor','Keyboard'], 'BoundingBox', True)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in extrude.mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(extrude.mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
def showObjectWithinVolume(self, req):
res = call_get_objects_within_volume(req.reference_mesh, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
array = MarkerArray()
pose = ROSPoseStamped()
pose.header.frame_id = 'world'
for polygon in req.reference_mesh.polygons:
poly = Polygon()
for point in polygon.vertex_indices:
poly.points.append(req.reference_mesh.vertices[point])
geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly)
array.markers.append(geo_marker)
id = 0
for m in array.markers:
m.header.frame_id = 'world'
m.id = id
id += 1
self.volume_query_marker.publish(array)
return res
def findObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, ["Chair"], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
self.area_query_marker.publish(polygon_stamped)
return res
def copyObjectWithinPolygonCb(self, polygon_stamped):
res = call_get_objects_within_area(polygon_stamped.polygon, [], "Position2D", fully_within = False)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
print 'COPY'
copy_res = call_copy_object_instances(ids)
if len( copy_res.ids ) > 1:
print 'copy_res', copy_res
obj_res = call_get_object_instances( [copy_res.ids[0]] )
print 'bind to ', obj_res.objects[0].name
frame = obj_res.objects[0].name
for id in copy_res.ids:
if id != obj_res.objects[0].id:
print 'bind', id, 'to', obj_res.objects[0].id
call_change_frame(id, frame, False)
call_activate_objects( copy_res.ids )
call_refresh_objects( copy_res.ids )
return res
def showObjectWithinArea(self, req):
res = call_get_objects_within_area(req.reference_polygon, req.target_object_types, req.target_object_geometry_type, req.fully_within)
ids=[]
for pair in res.pairs:
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
call_activate_objects( ids )
polygon_stamped = PolygonStamped()
polygon_stamped.header.frame_id = "world"
polygon_stamped.polygon = req.reference_polygon
self.area_query_marker.publish(polygon_stamped)
return res
def showDistanceBetweenObjects(self, req):
res = call_get_distance_between_objects(req.reference_object_types, req.reference_object_geometry_type,
req.target_object_types, req.target_object_geometry_type,
req.min_range, req.max_range,
req.sort_descending, req.max_distance, return_points = True)
array = MarkerArray()
lines = Marker()
lines.header.frame_id = "world"
lines.header.stamp = rospy.get_rostime()
lines.ns = "lines"
lines.id = 0
lines.type = 5 #line list
lines.scale.x = 0.05
lines.scale.y = 0.0
lines.scale.z = 0.0
lines.color.r = 0.75
lines.color.g = 0.750
lines.color.b = 0.750
lines.color.a = 1.0
ids =[]
for pair in res.pairs:
lines.points += pair.min_dist_line
lines.points += pair.max_dist_line
if pair.reference_id not in ids:
ids.append(pair.reference_id)
if pair.target_id not in ids:
ids.append(pair.target_id)
lines.colors.append(lines.color)
text = Marker()
text.header.frame_id = "world"
text.header.stamp = rospy.get_rostime()
text.ns = "distances"
text.id = 0
text.type = 9 #line list
text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2
text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2
text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05
text.scale.z = 0.2
text.color.r = 1.0
text.color.g = 1.0
text.color.b = 1.0
text.color.a = 1.0
text.text = str(round(pair.min_dist,3))
array.markers.append(text)
array.markers.append(lines)
id = 0
for m in array.markers:
m.id = id
id += 1
self.range_query_marker.publish(array)
call_activate_objects( ids )
return res
def list_active_objects(self, msg):
for key in self.objects.keys():
active = active_objects[key]
print 'ID :', active.id
print 'TYPE :', active.object.description.type
print 'NAME :', active.object.name
print 'ALIAS :', active.object.alias
print 'POSE :', active.object.pose
class PlaceObject:
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service(
"/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception
)
self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action)
self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty)
self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces)
self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(
self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction
)
self.grasp_setup_client.wait_for_server()
def do_perception(self, req):
rospy.loginfo("[place_object] Perceiving table...")
# capture a few clouds
rate = rospy.Rate(5)
for i in range(5):
self.pc_capture_srv()
rate.sleep()
# segment surfaces
self.surfaces = self.table_seg_srv().surfaces
for i in range(len(self.surfaces)):
self.surfaces[i].color.r = 256
self.surfaces[i].color.g = 0
self.surfaces[i].color.b = 256
self.surfaces[i].color.a = 256
resp = PerceiveButtonsResponse()
resp.buttons = self.surfaces
return resp
def do_action(self, req):
rospy.loginfo("[place_object] Table clicked, placing object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to place setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating place goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = False
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
grasp_goal.x = place_pt.point.x
grasp_goal.y = place_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# place object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[place_object] Place result: %s" % result)
obj_in_hand = result.grasp_result == "Object placed"
# go back to grasp setup
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()
class MoveGroupInterface(object):
## @brief Constructor for this utility
## @param group Name of the MoveIt! group to command
## @param frame Name of the fixed frame in which planning happens
## @param listener A TF listener instance (optional, will create a new one if None)
## @param plan_only Should we only plan, but not execute?
def __init__(self, group, frame, listener=None, plan_only=False):
self._group = group
self._fixed_frame = frame
self._action = actionlib.SimpleActionClient('move_group',
MoveGroupAction)
self._action.wait_for_server()
if listener == None:
self._listener = TransformListener()
else:
self._listener = listener
self.plan_only = plan_only
self.planner_id = None
self.planning_time = 15.0
def get_move_action(self):
return self._action
## @brief Move the arm to set of joint position goals
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
## @brief Move the arm, based on a goal pose_stamped for the end effector.
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
## @brief Sets the planner_id used for all future planning requests.
## @param planner_id The string for the planner id, set to None to clear
def setPlannerId(self, planner_id):
self.planner_id = str(planner_id)
## @brief Set default planning time to be used for future planning request.
def setPlanningTime(self, time):
self.planning_time = time
class GraspObjButton:
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_objects",
PerceiveButtons,
self.do_perception)
self.action_srv = rospy.Service("/clickable_world/grasp_object",
ButtonAction,
self.do_action)
self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect",
ObjectButtonDetect)
self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction)
self.grasp_setup_client.wait_for_server()
def do_perception(self, req):
rospy.loginfo("[grasp_obj_button] Perceiving object buttons...")
# perceive object on table
self.objects = self.obj_seg_srv().objects
for i in range(len(self.objects)):
self.objects[i].color.r = 256
self.objects[i].color.g = 0
self.objects[i].color.b = 0
self.objects[i].color.a = 256
# return object buttons
resp = PerceiveButtonsResponse()
resp.buttons = self.objects
return resp
def do_action(self, req):
rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to grasp setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating grasp goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = True
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP
grasp_goal.x = grasp_pt.point.x
grasp_goal.y = grasp_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# grasp object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result)
obj_in_hand = result.grasp_result == "Object grasped"
# go back to grasp setup
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()
class NavHeadController:
def __init__(self):
self.last_vel_time = rospy.Time(0)
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback)
self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback)
def velCallback(self, msg):
vel = msg.linear.x * msg.linear.x
vel += msg.linear.y * msg.linear.y
vel += msg.angular.z * msg.angular.z
if vel > 0.0:
self.last_vel_time = rospy.Time.now()
def planCallback(self, msg):
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
point = [1, 0, 0, 1]
M = R*point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0,0]
p.point.y += pose_stamped.pose.position.y + M[1,0]
p.point.z += pose_stamped.pose.position.z + M[2,0]
# transform to base_link
p = self.listener.transformPoint("base_link", p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
def __init__(self):
self.setup_node()
self.server = InteractiveMarkerServer("semap_environment")
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
