def __init__(self, parent, id):
the_size = (700, 270)
wx.Frame.__init__(self,
parent,
id,
'Magician Control Panel',
pos=(250, 100))
self.panel = wx.Panel(self)
font = self.panel.GetFont()
font.SetPixelSize((10, 20))
self.panel.SetFont(font)
self.listener = tf.TransformListener()
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('magician_arm')
self.elfin_basic_api_ns = 'magician_background/'
self.joint_names = self.group.get_active_joints()
self.active_joints = [0, 1, 3, 5]
self.ref_link_name = self.group.get_planning_frame()
self.end_link_name = self.group.get_end_effector_link()
self.js_display = [0] * 4 # joint_states
self.jm_button = [0] * 4 # joints_minus
self.jp_button = [0] * 4 # joints_plus
self.js_label = [0] * 4 # joint_states
self.ps_display = [0] * 4 # pcs_states
self.pm_button = [0] * 4 # pcs_minus
self.pp_button = [0] * 4 # pcs_plus
self.ps_label = [0] * 4 # pcs_states
self.display_init()
self.call_teleop_joint = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'joint_teleop', SetInt16)
self.call_teleop_joint_req = SetInt16Request()
self.call_teleop_cart = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'cart_teleop', SetInt16)
self.call_teleop_cart_req = SetInt16Request()
self.call_teleop_stop = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'stop_teleop', SetBool)
self.call_teleop_stop_req = SetBoolRequest()
self.call_stop = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'stop_teleop', SetBool)
self.call_stop_req = SetBoolRequest()
self.call_stop_req.data = True
self.SetMinSize(the_size)
self.SetMaxSize(the_size)
rospy.Timer(rospy.Duration(nsecs=50000000), self.monitor_status)
def execute(self, userdata):
pub_audio.publish("Samana bag found. Aligning with it")
# Start object detection
rospy.loginfo("Object detection STARTED")
self.enable_detector_proxy(SetBoolRequest(True))
self.subscriber_enabled = True
counter_grab_area = 0
self.point_lock.fresh_updated(no_audio=True)
while True:
if self.point_lock.is_fresh() is False:
# Bag detection lost return fail
self.visualize_bag_point(
point=None) # Clear bag pose visualization
self.execute_end_sequence()
return "fail"
if self.bag_point_odom is None:
sleep(0.1)
continue
# Update bag point in base frame from memory of odom frame point
# bag_point = self.tf_point(self.bag_point_odom, self.base_frame, self.odom_frame)
bag_point = self.tf_point(self.point_lock.get_locked_point(),
self.base_frame, self.odom_frame)
if bag_point is None:
sleep(0.1)
continue
self.visualize_bag_point(bag_point, self.base_frame)
p = Point(bag_point[0], bag_point[1]) # Shapely bag point
if self.area_drive_back1.contains(
p) or self.area_drive_back2.contains(p):
self.drive(-0.05, 0) # Drive backwards
else:
goal_reached = self.go_to_point(p, self.pos_goal,
self.yaw_goal) # Align
# Check if goal reach for some time. If so break (return success)
if goal_reached:
counter_grab_area += 1
if counter_grab_area > 10:
break
else:
counter_grab_area = max(0, counter_grab_area - 2)
self.cmd_vel_rate.sleep()
# Stopped object detection
rospy.loginfo("Object detection STOPPED")
self.enable_detector_proxy(SetBoolRequest(False))
self.execute_end_sequence()
return "success"
def joy_cb(self, msg):
if msg.buttons[5] == 1: # Autonomous control
req = SetBoolRequest()
req.data = False
resp = self.anto_srv(req)
elif msg.buttons[4] == 1: # Human control
req = SetBoolRequest()
req.data = True
resp = self.anto_srv(req)
def joy_cb(self, msg):
if msg.buttons[1] == 1:
req = SetBoolRequest()
req.data = False
resp = self.anto_srv(req)
elif msg.buttons[3] == 1:
req = SetBoolRequest()
req.data = True
resp = self.anto_srv(req)
def run_mission(self):
fprint('Starting mission')
fprint('Assuming camera is facing gate')
fprint('Leveling off', msg_color="yellow")
yield self.sub_singleton.move.zero_roll_and_pitch().go()
fprint('Going down', msg_color="yellow")
yield self.sub_singleton.move.down(.7).go()
start_gate_enable = yield self.sub_singleton.nh.get_service_client(
'/vision/start_gate/enable', SetBool)
fprint('Turning on vision service')
yield start_gate_enable(SetBoolRequest(data=True))
start_gate_search = yield self.sub_singleton.nh.get_service_client(
'/vision/start_gate/pose', VisionRequest2D)
fprint("Searching for start gate pose")
start_gate_search_res = yield start_gate_search(
VisionRequest2DRequest(target_name=''))
if not start_gate_search_res.found:
fprint("Waiting a few seconds and trying again:")
yield self.sub_singleton.sleep(3)
start_gate_search_res = yield start_gate_search(
VisionRequest2DRequest(target_name=''))
# This is to reset the buffer
yield start_gate_enable(SetBoolRequest(data=False))
yield start_gate_enable(SetBoolRequest(data=True))
if not start_gate_search_res.found:
fprint("Running search pattern")
while not self.FOUND_START_GATE and start_gate_search_res.pose.x == 0:
fprint(self.FOUND_START_GATE)
yield self.sub_singleton.sleep(1)
fprint("AGAIN - Searching for start gate pose")
self.start_gate_find()
yield self.search_pattern()
fprint("Found start gate: " + str(start_gate_search_res.pose))
start_gate_distance_request = yield self.sub_singleton.nh.get_service_client(
'/vision/start_gate/distance', BMatrix)
start_gate_distance = yield start_gate_distance_request(
BMatrixRequest())
fprint("Distance: " + str(start_gate_distance.B[0]))
yield self.align_for_dummies(start_gate_search_res)
fprint("YOLO -- MOVING FORWARD")
'''
while(self.FOUND_START_GATE):
self.start_gate_find()
yield self.sub_singleton.move.forward(.3).zero_roll_and_pitch().go(speed=self.SPEED)
'''
yield self.sub_singleton.move.forward(start_gate_distance.B[0] +
0.5).zero_roll_and_pitch().go(
speed=self.SPEED)
fprint("Finished")
def arm(self, armed):
try:
self.arm_proxy(SetBoolRequest(armed))
except rospy.ServiceException:
rospy.logwarn_throttle(
'[Motion Utils] Cannot arm/disarm: Arm server not responding. (This could be due '
'to running in simulation)')
def run(self, parameters):
'''
TODO: check for new objects in background, cancel move
somefucking how handle case where gates litteraly loop back and head the other direction
'''
self.objects_passed = set()
# Wait a bit for PCDAR to get setup
yield self.nh.sleep(3.0)
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=True))
yield self.prepare_to_enter()
yield self.wait_for_task_such_that(lambda task: task.state =='running')
yield self.move.forward(7.0).go()
while not (yield self.do_next_gate()):
pass
self.send_feedback('Exiting last gate!! Go NaviGator')
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=False))
def set_ring_light(self, value=True):
req = SetBoolRequest()
req.data = value
try:
self.ring_light_srv.call(req)
except:
rospy.logwarn('ring_light_switch not available')
def lock(self):
"""
@brief Lock the ontology server mutex to signal exclusive access
Note: locking is NOT mandatory and all functions will still work without locking the server
"""
return self._call(self._lock, SetBoolRequest(True)).success
def run(self, parameters):
p1 = BoolParameter(name='associator_forget_unseen', value=True)
p2 = DoubleParameter(name='cluster_tolerance_m', value=0.3)
p3 = DoubleParameter(name='associator_max_distance', value=0.25)
p4 = IntParameter(name='cluster_min_points', value=1)
p5 = IntParameter(name='persistant_cloud_filter_min_neighbors',
value=1)
yield self.pcodar_set_params(bools=[p1],
doubles=[p2, p3],
ints=[p4, p5])
# TODO: use PCODAR amnesia to avoid this timing fiasco
yield self.wait_for_task_such_that(
lambda task: task.state in ['running'])
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=True))
objects = {}
while True:
task_info = yield self.task_info_sub.get_next_message()
new_objects, positions = yield self.get_object_map()
position_enu = (yield self.tx_pose)[0]
for key in new_objects:
if key not in objects:
self.send_feedback('NEW object {} {}'.format(
key, new_objects[key]))
yield self.announce_object(key, new_objects[key],
positions[key], position_enu)
elif objects[key] != new_objects[key]:
self.send_feedback('{} changed from {} to {}'.format(
key, objects[key], new_objects[key]))
yield self.announce_object(key, new_objects[key],
positions[key], position_enu)
objects = new_objects
def unlock(self):
"""
@brief Unlocks the ontology server mutex
@return True if unlock service call was successfull
"""
return self._call(self._lock, SetBoolRequest(False)).success
def run(sub):
start_search = yield sub.nh.get_service_client('/vision/buoys/search',
SetBool)
yield start_search(SetBoolRequest(data=True))
print "BUOY MISSION - Executing search pattern"
yield search_again(sub)
ret = None
this_try = 0
while ret is None:
ret = yield bump_buoy(sub, 'red')
yield search_again(sub)
this_try += 1
if this_try > MAX_TRIES:
defer.returnValue(None)
print ret
yield sub.move.backward(2).go(speed=SPEED)
ret = None
this_try = 0
while ret is None:
ret = yield bump_buoy(sub, 'green')
yield search_again(sub)
this_try += 1
if this_try > MAX_TRIES:
defer.returnValue(None)
def execute(self,userdata):
userdata.slaves=dict()
for base in self.__base_namespaces:
userdata.slaves[base]=dict()
#Load reference vectors as poses
try:
ref=rospy.get_param(base+"/slave_controller/reference")
ref_pose=PoseStamped()
ref_pose.pose.position.x=ref[0]
ref_pose.pose.position.y=ref[1]
ref_pose.pose.position.z=0.0
quat=transformations.quaternion_from_euler(0.0,0.0,ref[2])
ref_pose.pose.orientation.x=quat[0]
ref_pose.pose.orientation.y=quat[1]
ref_pose.pose.orientation.z=quat[2]
ref_pose.pose.orientation.w=quat[3]
userdata.slaves[base]["reference"]=ref_pose
except:
return "references_error"
if self.__enable_manipulator:
req=SetBoolRequest()
req.data=False
self.__gripper_clients.call(req)
self.__formation_disabler.call(EmptyRequest())
self.__arm_controller.call(self.__arm_request)
return "startup_done"
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('arm_base_printing')
self.robot = moveit_commander.RobotCommander()
# Initialize moveit scene interface (woeld surrounding the robot)
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('robot')
self.msg_prit = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
self.add_three_box_obstacle()
point_list = []
for i in range(11):
point_list.append((i * 0.2 - 1, 3, 0.1))
print(point_list)
self.print_pointlist(point_list)
def set_joint_torque(self, state):
req = SetBoolRequest()
req.data = state
try:
self.set_joint_torque_client(req)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def run(self): req = TriggerRequest() if self.sensor_states[self.BUTTON_G1] == True: self.trigger_button_g1(req) if self.sensor_states[self.BUTTON_R1] == True: self.trigger_button_r1(req) if self.sensor_states[self.BUTTON_G2] == True: self.trigger_button_g2(req) if self.sensor_states[self.BUTTON_R2] == True: self.trigger_button_r2(req) req_state = SetBoolRequest() req_state.data = self.sensor_states[self.CELL_1] self.state_cell_1(req_state) req_state.data = self.sensor_states[self.CELL_2] self.state_cell_2(req_state) req_state.data = self.sensor_states[self.CELL_3] self.state_cell_3(req_state)
def __init__(self,
continuous_estimation=False,
joint_states_topic="/joint_states",
visual_tags_enabled=True):
super(Estimation, self).__init__(context="estimation")
self.locked_joints = []
self.tf_pub = TransformBroadcaster()
self.tf_root = "world"
self.mutex = Lock()
self.robot_name = rospy.get_param("~robot_name", "")
self.last_stamp_is_ready = False
self.last_stamp = rospy.Time.now()
self.last_visual_tag_constraints = list()
self.current_stamp = rospy.Time.now()
self.current_visual_tag_constraints = list()
self.visual_tags_enabled = visual_tags_enabled
self.continuous_estimation(SetBoolRequest(continuous_estimation))
self.estimation_rate = 50 # Hz
self.subscribers = ros_tools.createSubscribers(self, "/agimus",
self.subscribersDict)
self.publishers = ros_tools.createPublishers("/agimus",
self.publishersDict)
self.services = ros_tools.createServices(self, "/agimus",
self.servicesDict)
self.joint_state_subs = rospy.Subscriber(joint_states_topic,
JointState,
self.get_joint_state)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('arm_base_printing')
self.robot = moveit_commander.RobotCommander()
# Initialize moveit scene interface (woeld surrounding the robot)
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('robot')
self.arm_group = moveit_commander.MoveGroupCommander('arm')
self.msg_prit = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=10)
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
self.printing_number = 0
# self.add_three_box_obstacle()
point_list = []
# for i in range(11):
# point_list.append((i * 0.2 -1, 3, 0.1))
# print(point_list)
###Add obstacle
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 2
box_pose.pose.position.y = 0.5
box_pose.pose.position.z = 0.1
box_name = "wall"
self.scene.add_box(box_name, box_pose, size=(0.2, 0.2, 0.2))
rospy.sleep(0.5)
point_list = self.get_circle_point((2, 2), 1)
point_list_ob = self.get_circle_point((2, 2), 1, height=0.05)
print(point_list_ob)
# self.print_list_visualize(point_list_ob)
self.print_pointlist(point_list)
def step(self):
msg_str = self.recvStrFromProcessing()
if msg_str != "none":
msg_str = msg_str.split(',')
print msg_str
if msg_str[0] == 'GOALS_READY':
self.send_autonomy_goals_to_processing_service()
self.send_human_goals_to_processing_service()
elif msg_str[0] == 'GOALS_RESET':
msg = String("goals_reset")
self.goals_reset_pub.publish(msg)
#reset all goals.
self.reset_num_goals()
self.reset_autonomy_goals()
self.reset_human_goals()
self.send_autonomy_goals_to_processing_service()
self.send_human_goals_to_processing_service()
#reset autonomy and human robot pose.
self.reset_autonomy_robot()
self.reset_human_robot()
self.send_autonomy_robot_pose_to_processing_service()
self.send_human_robot_pose_to_processing_service()
elif msg_str[0] == 'BEGIN_TRIAL':
msg = String("begin_trial")
self.begin_trial_pub.publish(msg)
trigger_msg = SetBoolRequest()
trigger_msg.data = True
self.autonomy_node_trigger_trial(trigger_msg)
elif msg_str[0] == 'END_TRIAL':
trigger_msg = SetBoolRequest()
trigger_msg.data = False
self.autonomy_node_trigger_trial(trigger_msg)
msg = String("end_trial")
self.end_trial_pub.publish(msg)
elif msg_str[0] == 'ROBOT_READY':
msg = String("session_init")
self.session_init_pub.publish(msg)
self.send_autonomy_robot_pose_to_processing_service()
self.send_human_robot_pose_to_processing_service()
def stop_mission(self):
try:
req = SetBoolRequest()
req.data = False
self.activate_planner_service(req)
print "Mission has been stopped!"
except rospy.ServiceException, e:
print("Failed calling stop_mission service")
def click_buzzer_close(self):
client = rospy.ServiceProxy("/zxcar/buzzer", SetBool)
client.wait_for_service()
request = SetBoolRequest()
request.data = False
client.call(request)
client.close()
def click_led_open(self):
client = rospy.ServiceProxy("/zxcar/led", SetBool)
client.wait_for_service()
request = SetBoolRequest()
request.data = True
client.call(request)
client.close()
async def test_service() -> None:
async with init_master() as master:
async with init_node(
"some_server",
master_uri=master.xmlrpc_uri,
register_signal_handler=False,
configure_logging=False,
) as server:
async with anyio.create_task_group() as task_group:
task_group.start_soon(
server.create_server("/set_bool", SetBool,
service_cb).serve)
async with init_node(
"test_client",
master_uri=master.xmlrpc_uri,
register_signal_handler=False,
configure_logging=False,
) as node:
await wait_until_registered(master.xmlrpc_uri)
assert (master.registry.services["/set_bool"].api ==
server.tcpros_uri)
async with node.create_client("/set_bool",
SetBool) as client:
for _ in range(10):
result = await client.call(SetBoolRequest(True))
print(result)
assert result.success is True
assert result.message == "True"
async with node.create_client("/set_bool",
SetBool,
persistent=True) as client:
for _ in range(10):
result = await client.call(SetBoolRequest(True))
print(result)
assert result.success is True
assert result.message == "True"
task_group.cancel_scope.cancel()
assert "/set_bool" not in master.registry.services
assert not master.registry.nodes
assert not master.registry.services
def start_navigation(self):
rospy.loginfo("SRV: Start Navigation")
set_bool = SetBoolRequest()
set_bool.data = True
try:
srv = rospy.ServiceProxy('start_navigation', SetBool)
resp = srv(set_bool)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def set(self, data):
req = SetBoolRequest()
req.data = data
success = False
resp = None
while not success:
resp = self.proxy(req)
success = resp.success
self.rate.sleep()
return resp
def start_mission(self):
if self.state == State.FLYING_AUTO:
try:
req = SetBoolRequest()
req.data = True
self.activate_planner_service(req)
print "Mission has started!"
except rospy.ServiceException, e:
print("Failed calling start_mission service")
def _check_grasp_success():
go_home(); rospy.sleep(0.5)
calibrate_req = SetBoolRequest()
calibrate_res = calibrate_gripper_client(calibrate_req)
if not calibrate_res.success: return calibrate_res.success
check_grasp_success_request = check_grasp_successRequest()
# Get temporary pc and save file
_ = _get_pc(iteration, False, True)
check_grasp_success_request.pcd_str = check_grasp_path + "{:06}.pcd".format(iteration)
return check_grasp_success(check_grasp_success_request).is_success
def run(self, parameters):
self.objects_passed = set()
self.task_done = False
gates_passed = 0
# Wait a bit for PCDAR to get setup
yield self.nh.sleep(10.0)
yield self.move.set_orientation([0,0,0.7068,0.7073]).go()
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=True))
yield self.nh.sleep(4)
#yield self.move.forward(5).go()
while(not self.task_done or gates_passed == 5):
yield self.go_through_next_two_buoys()
gates_passed += 1
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=False))
def _handle_foodload(self, req):
rospy.logdebug("Request of LoadFood Recieved..Processing")
calibration_req = SetBoolRequest()
calibration_req.data = req.data
cal_response = self._calibrate_load_sensor_client(calibration_req)
rospy.logdebug("Calibration complete!")
response = SetBoolResponse()
response.success = cal_response.success
response.message = cal_response.message
return response
def execute(self, userdata):
arbitrary = input("pressToContinue")
realsense_request = SetBoolRequest()
kinect_request = String()
kinfu_reset_request = Empty()
kinfu_change_camera_request = EmptyRequest()
if self.action_ == 'realsense_off_kinect_on':
realsense_request.data = False
kinect_request.data = 'on'
rospy.set_param('/ros_kinfu/depth_image_topic',
'/kinect2/qhd/image_depth_rect')
rospy.set_param('/ros_kinfu/rgb_image_topic',
'/kinect2/qhd/image_color')
rospy.set_param('/ros_kinfu/camera_info_topic',
'/kinect2/qhd/camera_info')
rospy.set_param('/ros_kinfu/camera_frame_id',
'/kinect2_rgb_optical_frame')
rospy.set_param('/ros_kinfu/volume_size', 1.0)
elif self.action_ == 'realsense_on_kinect_off':
realsense_request.data = True
kinect_request.data = 'off'
rospy.set_param('/ros_kinfu/depth_image_topic',
'/realsense/depth/image_raw')
rospy.set_param('/ros_kinfu/rgb_image_topic',
'/realsense/rgb_depth_aligned/image_raw')
rospy.set_param('/ros_kinfu/camera_info_topic',
'/realsense/depth/camera_info')
rospy.set_param('/ros_kinfu/camera_frame_id',
'/camera_depth_optical_frame')
rospy.set_param('/ros_kinfu/volume_size', 0.85)
else:
return 'failed'
try:
response_realsense = self.realsense_service.call(realsense_request)
except:
return 'failed'
self.kinect_pub.publish(kinect_request)
while not self.camera_info_received:
print "Waiting for CameraInfo"
print self.camera_info_msg
self.objprop_change_camera_srv.call(self.camera_info_msg)
self.kinfu_reset_pub.publish(kinfu_reset_request)
# self.kinfu_change_camera_srv(kinfu_change_camera_request)
self.camera_info_received = False
if response_realsense.success:
return 'succeeded'
else:
return 'failed'