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 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 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 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 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 __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 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_led_open(self):
client = rospy.ServiceProxy("/zxcar/led", SetBool)
client.wait_for_service()
request = SetBoolRequest()
request.data = True
client.call(request)
client.close()
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 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 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 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 _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 resetNavigation(self):
self.start_navigation = False
self.set_path_succ = False
set_bool = SetBoolRequest()
set_bool.data = True
try:
srv = rospy.ServiceProxy('reset_goals', SetBool)
resp = srv(set_bool)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def srv_dive(self):
#rospy.wait_for_service('/set_path')
rospy.loginfo("SRV: Send diving")
set_bool = SetBoolRequest()
set_bool.data = True
try:
srv = rospy.ServiceProxy('dive', SetBool)
resp = srv(set_bool)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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'
def start_navigation(self, isTrue):
set_bool = SetBoolRequest()
if isTrue:
set_bool.data = True
else:
set_bool.data = False
try:
srv = rospy.ServiceProxy('start_navigation', SetBool)
resp = srv(set_bool)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def finish_diving(self):
#rospy.wait_for_service('/set_path')
rospy.loginfo("SRV: Send finish diving")
set_bool = SetBoolRequest()
set_bool.data = True
try:
srv = rospy.ServiceProxy('finish_diving', SetBool)
resp = srv(set_bool)
self.start_diving = False
return resp
except rospy.ServiceException, e:
print "Service call failed: %s"%e
async def main() -> None:
async with aioros.init_node("set_bool_client"):
async with aioros.create_client(
"/set_bool", SetBool, persistent=True
) as client:
request = SetBoolRequest(True)
while True:
result = await client.call(request)
request.data = not request.data
logger.info("Result: %s", result)
await anyio.sleep(1)
def srv_got_signal(self):
#rospy.wait_for_service('/set_path')
rospy.loginfo("SRV: Got signal")
set_bool = SetBoolRequest()
set_bool.data = False
try:
srv = rospy.ServiceProxy('no_signal', SetBool)
resp = srv(set_bool)
self.send_no_signal = False
return resp
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def execute(self, userdata):
if self.__enable_orientation_ff:
self.__orientation_init.call(EmptyRequest())
req = SwitchControllerRequest()
req.stop_controllers = []
req.start_controllers = ["compliance_controller"]
req.strictness = 2
self.__switcher.call(req)
grip_req = SetBoolRequest()
grip_req.data = True
self.__clients.call(grip_req)
return "linked"
def execute(self, userdata):
grip_req = SetBoolRequest()
grip_req.data = False
self.__clients.call(grip_req)
rospy.sleep(2)
req = SwitchControllerRequest()
req.stop_controllers = ["compliance_controller"]
req.start_controllers = ["position_joint_controller"]
req.strictness = 2
self.__switcher.call(req)
if self.__enable_orientation_ff:
self.__orientation_init.call(EmptyRequest())
return "released"
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 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 __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 __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 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 execute_toggle_lip(self):
# try to call the service
req = SetBoolRequest()
if self.action_ is 'lip_on':
req.data = True
elif self.action_ is 'lip_off':
req.data = False
try:
response = self.service.call(req)
rospy.loginfo('Lip has been toggled on/off')
return 'succeeded'
except Exception, e:
rospy.logerr('Service call to toggle_lip failed. Reason: \n %s' %
e)
return 'failed'
