def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
self.planning_scene_interface = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
self.surface_height = rospy.get_param('~surface_height')
self.surface_width = rospy.get_param('~surface_width')
self.surface_depth = rospy.get_param('~surface_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param(
'~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn(
"Didn't find any links to allow contacts... at param ~links_to_allow_contact"
)
else:
rospy.loginfo("Found links to allow contacts: " +
str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer('/pickup_pose',
PickUpPoseAction,
execute_cb=self.pick_cb,
auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer('/place_pose',
PickUpPoseAction,
execute_cb=self.place_cb,
auto_start=False)
self.place_as.start()
def __init__(self):
self.node_name = "PickAndPlaceServer"
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
# Get the object size
self.object_height = 0.1
self.object_width = 0.05
self.object_depth = 0.05
self.pick_pose = rospy.get_param('~pickup_marker_pose')
self.place_pose = rospy.get_param('~place_marker_pose')
rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
if not connected:
rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to pickup action server", self.node_name)
rospy.loginfo("%s: Waiting for place action server...", self.node_name)
self.place_ac = SimpleActionClient('/place', PlaceAction)
if not self.place_ac.wait_for_server(rospy.Duration(3000)):
rospy.logerr("%s: Could not connect to place action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to place action server", self.node_name)
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
self.scene = PlanningSceneInterface()
rospy.loginfo("Planning scene created.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()
def __init__(self):
self.query_shelf_systems_srv = rospy.Service(
'~query_shelf_systems', QueryShelfSystems,
self.query_shelf_systems_cb)
self.query_shelf_layers_srv = rospy.Service('~query_shelf_layers',
QueryShelfLayers,
self.query_shelf_layers_cb)
self.query_facings_srv = rospy.Service('~query_facings', QueryFacings,
self.query_facings_cb)
self.query_shelf_layer_detection_path_srv = rospy.Service(
'~query_detect_shelf_layers_path', QueryDetectShelfLayersPath,
self.query_detect_shelf_layers_path_cb)
self.query_facing_detection_path_srv = rospy.Service(
'~query_detect_facings_path', QueryDetectFacingsPath,
self.query_detect_facings_path_cb)
self.query_product_counting_path_srv = rospy.Service(
'~query_count_products_posture', QueryCountProductsPosture,
self.query_count_products_posture_cb)
self.finish_perception_srv = rospy.Service('~finish_perception',
FinishPerception,
self.finish_perception_cb)
self.query_reset_beliefstate_srv = rospy.Service(
'~reset_beliefstate', Trigger, lambda x: TriggerResponse())
self.detect_shelf_layer_as = SimpleActionServer(
'~detect_shelf_layers',
DetectShelfLayersAction,
execute_cb=self.detect_shelf_layers_cb,
auto_start=False)
self.detect_shelf_layer_as.register_preempt_callback(self.cancel_cb)
self.detect_facings_as = SimpleActionServer(
'~detect_facings',
DetectFacingsAction,
execute_cb=self.detect_facings_cb,
auto_start=False)
self.detect_facings_as.register_preempt_callback(self.cancel_cb)
self.count_products_as = SimpleActionServer(
'~count_products',
CountProductsAction,
execute_cb=self.count_products_cb,
auto_start=False)
self.count_products_as.register_preempt_callback(self.cancel_cb)
self.state_lock = Queue(1)
self.detect_shelf_layer_as.start()
self.detect_facings_as.start()
self.count_products_as.start()
self.set_ready(prefix='init')
self.set_names()
def __init__(self, name):
self.action_name = name
self.action_server = SimpleActionServer(self.action_name,
FibonacciAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_server.start()
def __init__(self, height, safe_dist):
self.quadrotor_height = height # Height of the motion to the ground
self.safe_dist = safe_dist # Distance the drone must maintain from a victim
# Create safe_land server
self.land_server = SimpleActionServer('safe_land_server',
ExecuteLandAction,
self.land_Callback, False)
self.server_feedback = ExecuteLandFeedback()
self.server_result = ExecuteLandResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.point_to_go = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
# quadrotor motion service
self.motors = rospy.ServiceProxy('enable_motors', EnableMotors)
self.motors.wait_for_service()
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
# Start trajectory server
self.land_server.start()
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for left finger
self.left_finger_frame = 'arm_left_finger_link'
self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
rospy.wait_for_message('finger_left_controller/state', JointControllerState)
# Initialize publisher & subscriber for right finger
self.right_finger_frame = 'arm_right_finger_link'
self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
rospy.wait_for_message('finger_right_controller/state', JointControllerState)
# Initialize action server
self.result = SmartArmGripperResult()
self.feedback = SmartArmGripperFeedback()
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)
# Reset gripper position
rospy.sleep(1)
self.reset_gripper_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/stop_audio_recording', StopAudioRecording)
self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning',
GraspPlanning)
self.get_solver_info_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik_solver_info',
GetKinematicSolverInfo)
self.get_ik_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik', GetPositionIK)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
self.find_cluster_bbox = rospy.ServiceProxy(
'/find_cluster_bounding_box', FindClusterBoundingBox)
self.posture_controller = SimpleActionClient(
'/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.attached_object_pub = rospy.Publisher(
'/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PutDownAtAction,
execute_cb=self.put_down_at,
auto_start=False)
def __init__(self, activate=True):
self._ns = rospy.get_namespace()
# Read configuration parameters
self._fb_rate = read_parameter(
self._ns + 'gripper_action_controller/publish_rate', 60.0)
self._min_gap = read_parameter(
self._ns + 'gripper_action_controller/min_gap', 0.0)
self._max_gap = read_parameter(
self._ns + 'gripper_action_controller/max_gap', 0.085)
self._min_speed = read_parameter(
self._ns + 'gripper_action_controller/min_speed', 0.013)
self._max_speed = read_parameter(
self._ns + 'gripper_action_controller/max_speed', 0.1)
self._min_force = read_parameter(
self._ns + 'gripper_action_controller/min_force', 40.0)
self._max_force = read_parameter(
self._ns + 'gripper_action_controller/max_force', 100.0)
# Configure and start the action server
self._status = CModelStatus()
self._name = self._ns + 'gripper_action_controller'
self._server = SimpleActionServer(self._name,
CModelCommandAction,
execute_cb=self._execute_cb,
auto_start=False)
rospy.Subscriber('status', CModelStatus, self._status_cb)
self._cmd_pub = rospy.Publisher('command', CModelCommand, queue_size=3)
working = True
if activate and not self._ready():
rospy.sleep(2.0)
working = self._activate()
if not working:
return
self._server.start()
rospy.loginfo(rospy.loginfo('%s: Started' % self._name))
def __init__(self, action_name, **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
# won't use default auto_start=True as recommended here: https://github.com/ros/actionlib/pull/60
self._action_server = SimpleActionServer(action_name, DetectSceneAction,
execute_cb=self._execute_cb, auto_start=False)
self._initialize(**kwargs)
self._action_server.start()
def __init__(self):
self.action_server = SimpleActionServer(
'move_base',
MoveBaseAction,
execute_cb=self.execute_callback,
auto_start=False
) # Simple action server will pretend to be move_base
# Movement goal state
self.goal = None # Is a goal set
self.valid_goal = False # Is this a valid goal
self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start)
self.current_goal_complete = False # Has the Bug algorithm told us it completed
self.position = None # move_base feedback reports the current direction
self.bugType = bugType.BUG2 # set the bug type to be used
# Service for the Bug algorithm to tell us it is done
self.bug_done_service = rospy.Service('/move_base_fake/is_bug_done/',
SetBool, self.callback_complete)
self.subscriber_odometry = rospy.Subscriber(
'odom/', Odometry, self.callback_odometry
) # We need to read the robots current point for the feedback
self.subscriber_simple_goal = rospy.Subscriber(
'/move_base_simple/goal/', PoseStamped, self.callback_simple_goal
) # Our return goal is done with /move_base_simple/goal/
self.goal_pub = rospy.Publisher(
'/move_base/goal/', MoveBaseActionGoal,
queue_size=10) # /move_base_simple/goal/ gets published here
self.action_server.start()
def __init__(self):
# action client
self._ac_gp = SimpleActionClient("global_planner_node_action",
GlobalPlannerAction)
self._ac_gp.wait_for_server()
self._ac_lp = SimpleActionClient("local_planner_node_action",
LocalPlannerAction)
self._ac_lp.wait_for_server()
# action servergoal
self._as = SimpleActionServer("nav_to_node_action",
NavToAction,
self.ExecuteCB,
auto_start=False)
# define goals to send
self.global_planner_goal_ = GlobalPlannerGoal()
self.local_planner_goal_ = LocalPlannerGoal()
self.new_goal_ = False
self.new_path_ = False
self.global_planner_error_code = -1
self.local_planner_error_code = -1
# new thread running the execution loop
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command',
Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal',
LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name +
'/set_speed',
SetSpeed,
persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState,
self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state',
JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action',
HokuyoLaserTiltAction,
self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def __init__(self):
self._poses = []
self._poses_idx = 0
self._client = SimpleActionClient('/bug2/move_to', MoveToAction)
self._action_name = rospy.get_name() + "/execute_path"
self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False)
self._server.start()
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/stop_audio_recording', StopAudioRecording)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
self.interpolated_ik_params_srv = rospy.ServiceProxy(
'/l_interpolated_ik_motion_plan_set_params',
SetInterpolatedIKMotionPlanParams)
self.interpolated_ik_srv = rospy.ServiceProxy(
'/l_interpolated_ik_motion_plan', GetMotionPlan)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.get_fk_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_fk', GetPositionFK)
self.trajectory_filter_srv = rospy.ServiceProxy(
'/trajectory_filter_unnormalizer/filter_trajectory',
FilterJointTrajectory)
self.trajectory_controller = SimpleActionClient(
'/l_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.attached_object_pub = rospy.Publisher(
'/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PushObjectAction,
execute_cb=self.push_object,
auto_start=False)
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def __init__(self):
"""
Make dictionary of dequeues.
Subscribe to set of topics defined by the yaml file in directory
Stream topics up to a given stream time, dump oldest messages when limit is reached
Set up service to bag n seconds of data default to all of available data
"""
self.successful_subscription_count = 0 # successful subscriptions
self.iteration_count = 0 # number of iterations
self.streaming = True
self.get_params()
if len(self.subscriber_list) == 0:
rospy.logwarn('No topics selected to subscribe to. Closing.')
rospy.signal_shutdown('No topics to subscribe to')
return
self.make_dicts()
self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC,
BagOnlineAction,
execute_cb=self.start_bagging,
auto_start=False)
self.subscribe_loop()
rospy.loginfo('Remaining Failed Topics: {}\n'.format(
self.get_subscriber_list(False)))
self._action_server.start()
def __init__(self):
server_ip = rospy.get_param('~overpass_server_ip')
server_port = rospy.get_param('~overpass_server_port')
ref_lat = rospy.get_param('~ref_latitude')
ref_lon = rospy.get_param('~ref_longitude')
building = rospy.get_param('~building')
global_origin = [ref_lat, ref_lon]
rospy.loginfo("Server " + server_ip + ":" + str(server_port))
rospy.loginfo("Global origin: " + str(global_origin))
rospy.loginfo("Starting servers...")
self.osm_topological_planner_server = SimpleActionServer(
'/osm_topological_planner', OSMTopologicalPlannerAction, self._osm_topological_planner, False)
self.osm_topological_planner_server.start()
osm_bridge = OSMBridge(
server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
path_planner = PathPlanner(osm_bridge)
path_planner.set_building(building)
self.osm_topological_planner_callback = OSMTopologicalPlannerCallback(
osm_bridge, path_planner)
rospy.loginfo(
"OSM topological planner server started. Listening for requests...")
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def __init__(self, name):
self._server = SimpleActionServer(name,
QueryAction,
execute_cb=self._execute_cb,
auto_start=False)
self._server.start()
rospy.loginfo('HMI server started on "%s"', name)
def __init__(self, name):
rospy.loginfo("Starting {}".format(name))
self._as = SimpleActionServer(name,
TopoNavAction,
self.execute_cb,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient("/waypoint_nav_node",
WayPointNavAction)
rospy.loginfo("Waiting for nav server")
self.client.wait_for_server()
self.nodes = set()
self.edges = defaultdict(list)
self.distances = {}
with open(rospy.get_param("~waypoint_yaml"), 'r') as f:
self.waypointInfo = yaml.load(f)
for waypoint, info in self.waypointInfo.items():
self.add_node(waypoint)
for edge in info["edges"]:
self.add_edge(waypoint, edge, 1.0)
self.waypointInfo[waypoint]["position"]["x"] *= 0.555
self.waypointInfo[waypoint]["position"]["y"] *= 0.555
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped,
self.pose_cb)
self._as.start()
rospy.loginfo("Started {}".format(name))
def __init__(self):
self.action_server = SimpleActionServer(
'move_base',
MoveBaseAction,
execute_cb=self.execute_callback,
auto_start=False
) # Simple action server will pretend to be move_base
# Movement goal state
self.goal = None # Is a goal set
self.valid_goal = False # Is this a valid goal
self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start)
self.current_goal_complete = False # Has the Bug algorithm told us it completed
self.position = None # move_base feedback reports the current direction
## TO DO!!
# Need a service provided by this node or something for the Bug algorithm to tell us it is done
# Bug service to start and stop Bug algorithm
# Bug service to set a new goal in Bug algorithm
# rospy.wait_for_service()
# rospy.wait_for_service()
self.subscriber_odometry = rospy.Subscriber(
'odom/', Odometry, self.callback_odometry
) # We need to read the robots current point for the feedback
self.subscriber_simple_goal = rospy.Subscriber(
'/move_base_simple/goal/', PoseStamped, self.callback_simple_goal
) # Our return goal is done with /move_base_simple/goal/
self.goal_pub = rospy.Publisher(
'/move_base/goal/', MoveBaseActionGoal,
queue_size=10) # /move_base_simple/goal/ gets published here
self.action_server.start()
def __init__(self, name="aggressive_gain_buff_action"):
self.action_name = name
self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False)
self.gain_buff_state = GainBuffState.ROUTE
self.chassis_state_ = 0
self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB)
self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode)
self.chassis_arrive_state = ChassisArriveState()
self.chassis_mode_ = 0
self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction)
rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...')
ret = self._ac_navto.wait_for_server()
rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!')
self.nav_to_error_code = -1
self.search_start_time = rospy.Time(0)
self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction)
rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...')
ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3))
rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!')
self.Look_n_move_error_code = -1
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def __init__(self, name):
self._action_name = name
self._as = SimpleActionServer(self._action_name,
ExecutePathAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("in action_server")
def __init__(self, namespace, action_spec, execute_cb):
self.goal_service = rospy.Service(namespace + '/get_goal_from_id',
GetTaskFromID, self.task_id_cb)
self.server = SimpleActionServer(namespace,
action_spec,
execute_cb,
auto_start=False)
self.server.start()
def __init__(self):
self.rate = rospy.Rate(10)
self.goto_server = SimpleActionServer('goto',
GotoAction,
execute_cb=self.goto_execute,
auto_start=False)
self.goto_server.start()
self.last_object = "base1"
def __init__(self):
self._default_focus_point = Point(1, 0, 1.05)
self._down_focus_point = Point(0.5, 0, 0.5)
self._target_focus_point = Point(1, 0, 1.05)
self._current_focus_point = Point(1, 0, 1.05)
self._current_gaze_action = GazeGoal.LOOK_FORWARD
self._prev_gaze_action = self._current_gaze_action
self._prev_target_focus_point = array(self._default_focus_point)
# Some constants
self._no_interrupt = [GazeGoal.NOD,
GazeGoal.SHAKE, GazeGoal.LOOK_DOWN]
self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)]
self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)]
self._n_nods = 5
self._n_shakes = 5
self._nod_counter = 5
self._shake_counter = 5
self._face_pos = None
self._glance_counter = 0
self.gaze_goal_strs = {
GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD',
GazeGoal.FOLLOW_EE: 'FOLLOW_EE',
GazeGoal.GLANCE_EE: 'GLANCE_EE',
GazeGoal.NOD: 'NOD',
GazeGoal.SHAKE: 'SHAKE',
GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE',
GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT',
GazeGoal.LOOK_DOWN: 'LOOK_DOWN',
GazeGoal.NONE: 'NONE',
}
## Action client for sending commands to the head.
self._head_action_client = SimpleActionClient(
'/head_controller/point_head', PointHeadAction)
self._head_action_client.wait_for_server()
rospy.loginfo('Head action client has responded.')
self._head_goal = PointHeadGoal()
self._head_goal.target.header.frame_id = 'base_link'
self._head_goal.min_duration = rospy.Duration(1.0)
self._head_goal.target.point = Point(1, 0, 1)
## Service client for arm states
self._tf_listener = TransformListener()
## Server for gaze requested gaze actions
self._gaze_action_server = SimpleActionServer(
'gaze_action', GazeAction, self._execute_gaze_action, False)
self._gaze_action_server.start()
self._is_action_complete = True
rospy.Service('get_current_gaze_goal', GetGazeGoal,
self._get_gaze_goal)
def __init__(self, action_name, action_type):
self.goal_queue = Queue(1)
self.result_queue = Queue(1)
self.lock = Blackboard().lock
self.action_name = action_name
self.cancel_next_goal = False
self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False)
self._as.register_preempt_callback(self.cancel_cb)
self._as.start()
def __init__(self, action_name, timeout_s=10., **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
self._action_server = SimpleActionServer(action_name,
DetectObjectsAction,
execute_cb=self._execute_cb,
auto_start=False)
self._timeout_s = timeout_s
self._initialize(**kwargs)
self._action_server.start()
def __init__(self):
rospy.loginfo("inside __init__")
self.Server = "/turtle_thing"
print(self.Server)
self.turtle_server = SimpleActionServer(self.Server,
Turtle_positionAction,
execute_cb=self.execute_cb,
auto_start=False)
self.turtle_server.start()
