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.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):
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):
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):
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()
class OmniPoseFollower(object):
def __init__(self):
self.server = SimpleActionServer(
'/whole_body_controller/refills_finger/follow_joint_trajectory',
FollowJointTrajectoryAction,
self.execute_cb,
auto_start=False)
self.state_pub = rospy.Publisher('refills_finger/state',
JointTrajectoryControllerState,
queue_size=10)
self.js_sub = rospy.Subscriber('refills_finger/joint_states',
JointState,
self.js_cb,
queue_size=10)
self.server.start()
def js_cb(self, data):
msg = JointTrajectoryControllerState()
msg.joint_names = data.name
self.state_pub.publish(msg)
def execute_cb(self, data):
"""
:type data: FollowJointTrajectoryGoal
:return:
"""
self.server.set_succeeded()
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, 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()
class GiveVoucher(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = SimpleActionServer(name,
GiveVoucherAction,
self.execute_cb,
auto_start=False)
self.logo_app = rospy.get_param("~logo_app",
"showmummerlogo-a897b8/behavior_1")
client = MongoClient(rospy.get_param("~db_host", "localhost"),
int(rospy.get_param("~db_port", 62345)))
self.db_name = rospy.get_param("~db_name", "semantic_map")
self.db = client[self.db_name]
self.collection_name = rospy.get_param("~collection_name", "idea_park")
self.semantic_map_name = rospy.get_param("~semantic_map_name")
self._as.start()
rospy.loginfo("... done")
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
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, 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):
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):
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):
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):
"""
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()
class MoveBase(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.odom_topic = rospy.get_param("~odom_topic",
"/naoqi_driver_node/odom")
self.listener = tf.TransformListener()
self.robot_pose = None
rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb)
self.target_frame = rospy.get_param("~target_frame", "base_link")
# self.pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
self._as = SimpleActionServer(name,
MoveBaseAction,
self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("... done")
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
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, 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, 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):
# 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, 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
## 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):
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, 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, 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, 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):
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()
class PipolFollower():
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction,
execute_cb = self.execute_cb,
preempt_callback = self.preempt_cb,
auto_start = False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def execute_cb(self, goal):
print "goal is: " + str(goal)
# helper variables
success = True
# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class ReadyArmActionServer:
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 initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
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)
self.scene = 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 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()
class ScanTableActionServer:
"""
Scan Table Mock
Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table)
"""
def __init__(self):
a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_scan_table)
self.s.start()
def scan_table_cb(self, req):
rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.')
self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()
class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while(not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
def __init__(self):
self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0)
self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02)
gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action')
self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
#
# while not rospy.is_shutdown():
# try:
# self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0))
# break
# except ROSException as e:
# rospy.loginfo('Waiting for %s action' % gripper_action_name)
# except:
# rospy.logerr('Unexpected error')
# raise
rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name)
query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status')
self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status)
posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action')
self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False)
self.action_server.start()
rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name)
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):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
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, name):
self.fullname = name
self.jointPositions = []
self.jointVelocities = []
self.jointAccelerations = []
self.jointNamesFromConfig = []
for configJoint in configJoints:
self.jointNamesFromConfig.append(configJoint['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
if 'mimic_joints' in configJoint:
for mimic in configJoint['mimic_joints']:
self.jointNamesFromConfig.append(mimic['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
self.pointsQueue = []
self.lastTimeFromStart = 0.0
startPositions = [0.0, -0.9, -0.9, 0.0, 0.0, 0.0]
startVelocities = [0.5]*6
self.jointPositions[1] = -0.9
dynamixelChain.move_angles_sync(ids[:6], startPositions, startVelocities)
# self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
self.server = SimpleActionServer(self.fullname,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 5 # Number of joints to manage
self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME + 'server', anonymous=True)
# Initialize publisher & subscriber for shoulder pan
self.shoulder_pan_frame = 'arm_shoulder_tilt_link'
self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointState)
# Initialize publisher & subscriber for arm tilt
self.arm_tilt_frame = 'arm_pan_tilt_bracket'
self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64)
rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt)
rospy.wait_for_message('arm_tilt_controller/state', JointState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_bracket'
#self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointState)
# Initialize publisher & subscriber for wrist pan
self.wrist_pan_frame = 'wrist_pan_link'
self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64)
rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan)
rospy.wait_for_message('wrist_pan_controller/state', JointState)
# Initialize publisher & subscriber for wrist tilt
self.wrist_tilt_frame = 'wrist_tilt_link'
self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64)
rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt)
rospy.wait_for_message('wrist_tilt_controller/state', JointState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = RobbieArmResult()
self.feedback = RobbieArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback)
# Reset arm position
rospy.sleep(1)
self.reset_arm_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
class ReemTabletopManipulationMock():
def __init__(self):
a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_grasp_target_action)
self.s.start()
def grasp_target_action_cb(self, req):
rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID)
res = GraspTargetResult()
res.detectionResult = TabletopDetectionResult()
res.detectionResult.models = [DatabaseModelPoseList()]
res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID
res.detectionResult.models[0].model_list[0].model_id = req.databaseID
self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)
class BaseRotate():
def __init__(self):
self._action_name = 'base_rotate'
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#initialize this client
self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def run(self, goal):
rospy.loginfo('Rotating base')
count = 0
r = rospy.Rate(10)
while count < 70:
if self._as.is_preempt_requested():
self._as.set_preempted()
return
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 1.0)
self._base_publisher.publish(twist_msg)
count = count + 1
r.sleep()
self._as.set_succeeded()
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction,
execute_cb = self.execute_cb,
preempt_callback = self.preempt_cb,
auto_start = False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def __init__(self, name):
self.fullname = name
self.goalAngle = 0.0
self.actualAngle = 0.0
self.failureState = False
dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5])
self.server = SimpleActionServer(
self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def __init__(self, name):
self.fullname = name
self.currentValue = 0.0
gripperTopic = '/arm_1/gripper_controller/position_command'
self.jppub = rospy.Publisher(gripperTopic, JointPositions)
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def __init__(self, name):
self.fullname = name
self.currentAngle = 0.0
# dynamixelChain.move_angle(7, 0.0, 0.5)
dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5])
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
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.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity)
self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64)
self.action_server = SimpleActionServer(ACTION_NAME,
ShakePitchObjectAction,
execute_cb=self.shake_pitch_object,
auto_start=False)
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def __init__(self):
self._action_name = 'base_rotate'
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#initialize this client
self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
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.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
DropObjectAction,
execute_cb=self.drop_object,
auto_start=False)
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus)
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.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, LiftObjectAction, execute_cb=self.lift_object, auto_start=False
)
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
class axGripperServer:
def __init__(self, name):
self.fullname = name
self.currentAngle = 0.0
# dynamixelChain.move_angle(7, 0.0, 0.5)
dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5])
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
# dynamixelChain.move_angle(7, 0.1, 0.5)
rospy.sleep(0.1)
print jPositions[4]
attempts = 0
while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20:
rospy.sleep(0.1)
print jPositions[4]
attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
class axGripperServer:
def __init__(self, name):
self.fullname = name
self.goalAngle = 0.0
self.actualAngle = 0.0
self.failureState = False
dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5])
self.server = SimpleActionServer(
self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
attempts = 0
for i in range(10):
rospy.sleep(0.1)
attempts += 1
# while ... todo: add some condition to check on the actual angle
# rospy.sleep(0.1)
# print jPositions[4]
# attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
def checkFailureState(self):
if self.failureState:
print "I am currently in a failure state."
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def __init__(self, ip_robot):
self.ip_robot = ip_robot
# pan tilt
self.joint_states = [None, None]
self.js_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=5)
self.as_ = SimpleActionServer('/robot_controller',
JointTrajectoryAction,
auto_start=False,
execute_cb=self.goal_cb)
self.as_.start()
self.go_to_position(pan=0.0, tilt=0.0)
rospy.Timer(rospy.Duration(0.02), self.js_pub_cb, oneshot=False)
rospy.loginfo("We are started!")
def __init__(self, name, configJoints):
self.fullname = name
self.jointNames = []
for configJoint in configJoints:
self.jointNames.append(configJoint["name"])
self.jointNames = self.jointNames[:4]
self.failureState = False
self.goalPositions = [0.0, -0.9, 0.0, 0.0]
self.goalVelocities = [0.5] * 4
self.move_chain()
# todo: add method for checking arm's actual positions
self.actualPositions = [0.0, -0.9, 0.0, 0.0]
self.server = SimpleActionServer(
self.fullname, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
