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 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 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 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_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'
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 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 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 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 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 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 _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 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
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 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
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 activate_joystick(self, activate):
"""Requests joystick activation/deactivation from joystick_guidance
Args:
activate (bool): True activates joystick, False deactivates
"""
request = SetBoolRequest()
request.data = activate
try:
self.activate_joystick_service(request)
return True
except rospy.ServiceException as exc:
rospy.logerr(
"Joystick activation service did not process request: " +
str(exc))
return False
def go_home_cb(self, req):
resp = SetBoolResponse()
torque_require = SetBoolRequest()
torque_require.data = True
self.torque_enable_cb(torque_require)
self.action_client.wait_for_server()
self.action_goal.trajectory.header.stamp.secs = 0
self.action_goal.trajectory.header.stamp.nsecs = 0
point_tmp = JointTrajectoryPoint()
position_tmp = []
for i in xrange(len(self.controller_names)):
position_tmp.append(0)
point_tmp.positions = position_tmp
point_tmp.time_from_start.secs = int(self.go_home_time)
print len(self.action_goal.trajectory.points)
self.action_goal.trajectory.points.append(point_tmp)
self.action_client.send_goal(self.action_goal)
self.action_client.wait_for_result()
self.action_goal.trajectory.points = []
resp.success = True
return resp
def enable_eStop(self):
eStop_req = SetBoolRequest()
eStop_req.data = True
self.trigger_service(eStop_req)
def disable_eStop(self):
eStop_req = SetBoolRequest()
eStop_req.data = False
self.trigger_service(eStop_req)
def __init__(self, name=None):
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# Init moveit group
self.group = moveit_commander.MoveGroupCommander('robot')
self.arm_group = moveit_commander.MoveGroupCommander('arm')
self.base_group = moveit_commander.MoveGroupCommander('base')
self.scene = moveit_commander.PlanningSceneInterface()
self.sce = moveit_python.PlanningSceneInterface('odom')
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
sub_waypoint_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.waypoint_execution_cb)
sub_movegroup_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.move_group_execution_cb)
# sub_movegroup_status = rospy.Subscriber('move_group/status', GoalStatusArray, self.move_group_execution_cb)
rospy.Subscriber("joint_states", JointState, self.further_ob_printing)
#########################################################################
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/joint_states", JointState, self.eef_callback)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.base_position_x = []
self.base_position_y = []
self.base_position_z = []
self.eef_position_x = []
self.eef_position_y = []
self.eef_position_z = []
self.position_time = []
self.name = name
self.js_0 = []
self.js_1 = []
self.js_2 = []
self.js_time = []
self.startime = datetime.datetime.now()
self.mkdir('experiment_data' + '/' + name)
#############################################################################
msg_print = SetBoolRequest()
msg_print.data = True
self.re_position_x = []
self.re_position_y = []
self.waypoint_execution_status = 0
self.move_group_execution_status = 0
self.further_printing_number = 0
self.pre_further_printing_number = 0
# initial printing number
self._printing_number = 0
self._further_printing_number = 0
self.future_printing_status = False
self.current_printing_pose = None
self.previous_printing_pose = None
self.target_list = None
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize time record
self.travel_time = 0
self.planning_time = 0
self.printing_time = 0
# 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.pub_rviz_marker = rospy.Publisher('/visualization_marker',
Marker,
queue_size=100)
self.remove_all_rviz_marker()
self.printing_number_rviz = 0
def _call(self, monitoring_on_off_flag):
req = SetBoolRequest()
req.data = monitoring_on_off_flag
resp = self._set_catesian_speed_srv(req)
return resp.success