def __init__(self, airskin_topic, bumper_topic, wrist_topic): while rospy.get_time() == 0.0: pass rospy.loginfo(rospy.get_caller_id() + ': starting up') self.wrist_sub = rospy.Subscriber(wrist_topic, Bool, self.wrist_callback) self.bumper_sub = rospy.Subscriber(bumper_topic, Bool, self.bumper_callback) self.airskin_sub = rospy.Subscriber(airskin_topic, Bool, self.airskin_callback) self.safety_pub = rospy.Publisher('/squirrel_safety', Safety, queue_size=10) self.safety_reset_sub = rospy.Subscriber('/squirrel_safety/reset', Bool, self.safety_reset_callback) self.controller_reset_ = rospy.ServiceProxy('/arm_controller/controller_manager/switch_controller', SwitchController) rate = rospy.Rate(10) self.safety_msg = Safety() self.safety_msg.emergency_state = False self.safety_msg.airskin_stop = False self.safety_msg.wrist_stop = False self.safety_msg.bumper_stop = False while not rospy.is_shutdown(): if self.safety_msg.wrist_stop or self.safety_msg.bumper_stop or self.safety_msg.airskin_stop: self.safety_msg.emergency_state = True req = SwitchControllerRequest() req.start_controllers = [] req.stop_controllers = ['joint_trajectory_controller'] req.strictness = 2 if self.controller_reset_(req): rospy.loginfo('Safety set.') else: rospy.logerror('Failed to set safety. Please kick it again or manually shut down and panic!') self.safety_pub.publish(self.safety_msg) self.safety_msg.emergency_state = False rate.sleep()
def switch_controllers(self, controllers_on, controllers_off, strictness=1): """ Give the controllers you wan to switch on or off. :param controllers_on: ["name_controler_1", "name_controller2",...,"name_controller_n"] :param controllers_off: ["name_controler_1", "name_controller2",...,"name_controller_n"] :return: """ rospy.wait_for_service(self.switch_service_name) try: switch_request_object = SwitchControllerRequest() switch_request_object.start_controllers = controllers_on switch_request_object.start_controllers = controllers_off switch_request_object.strictness = strictness switch_result = self.switch_service(switch_request_object) """ [controller_manager_msgs/SwitchController] int32 BEST_EFFORT=1 int32 STRICT=2 string[] start_controllers string[] stop_controllers int32 strictness --- bool ok """ rospy.logdebug("Switch Result==>" + str(switch_result.ok)) return switch_result.ok except rospy.ServiceException, e: print(self.switch_service_name + " service call failed") return None
def switch_controller(self, mode=None): req = SwitchControllerRequest() res = SwitchControllerResponse() req.start_asap = False req.timeout = 0.0 if mode == 'velocity': req.start_controllers = ['joint_group_vel_controller'] req.stop_controllers = ['scaled_pos_traj_controller'] req.strictness = req.STRICT elif mode == 'position': req.start_controllers = ['scaled_pos_traj_controller'] req.stop_controllers = ['joint_group_vel_controller'] req.strictness = req.STRICT else: rospy.logwarn('Unkown mode for the controller!') res = self.switch_controller_cli.call(req)
def switch_on_controller(self, controller_name): """Switches on the given controller stopping all other known controllers with best_effort strategy.""" srv = SwitchControllerRequest() srv.stop_controllers = ALL_CONTROLLERS srv.start_controllers = [controller_name] srv.strictness = SwitchControllerRequest.BEST_EFFORT result = self.switch_controllers_client(srv) self.assertTrue(result.ok)
def execute(self, userdata): req = SwitchControllerRequest() req.stop_controllers = ["position_joint_controller"] req.start_controllers = ["cartesian_controller"] req.strictness = 2 self.__switcher.call(req) srv = rospy.Service("~trigger", Empty, self.__callback__) while not self.__enable: rospy.sleep(self.__timeout) self.__enable = False srv.shutdown() req = SwitchControllerRequest() req.stop_controllers = ["cartesian_controller"] req.start_controllers = [] req.strictness = 2 self.__switcher.call(req) return "adjusted"
def start_ros_controllers(): start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER, SwitchController) req = SwitchControllerRequest() req.start_controllers.append("ur5_cartesian_velocity_controller_sim") req.start_controllers.append("joint_state_controller") req.start_controllers.append("ur5_joint_publisher") req.start_controllers.append("ur5_velocity_controller") req.strictness = req.BEST_EFFORT rospy.wait_for_service(SWITCH_CONTROLLER) res = start_controllers_client(req)
def start_ros_controllers(): start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER, SwitchController) req = SwitchControllerRequest() # this two controller is same as the ur joint trajectory controller req.start_controllers.append("ur5_cartesian_velocity_controller_sim") req.start_controllers.append("joint_state_controller") req.strictness = req.BEST_EFFORT rospy.wait_for_service(SWITCH_CONTROLLER) res = start_controllers_client(req)
def safety_reset_callback(self, msg): if msg.data == True: req = SwitchControllerRequest() req.stop_controllers = [] req.start_controllers = ['joint_trajectory_controller'] req.strictness = 2 if self.controller_reset_(req): rospy.loginfo('Safety released.') else: rospy.logerror('Failed to release safety. Please try again or manually restart both the controller and the safety node!')
def switch_to_traj_ctrl(self): ''' call service /controller_manager/switch_controller start trajectory ctrl, stop position + velocity ctrl ''' req = SwitchControllerRequest() req.start_controllers = self.trajectory_ctrler_start_stop_list[0] req.stop_controllers = self.trajectory_ctrler_start_stop_list[1] req.strictness = req.BEST_EFFORT self.switch_ctrler_srv(req) # to keep in memory the running controller self.running_ctler = 'trajectory' rospy.loginfo('Automatically switched to trajectory ctrl')
def _deactivate_controller(self, controller_name): if self._is_controller_running(controller_name): request = SwitchControllerRequest() request.stop_controllers = [controller_name] request.strictness = request.STRICT response = self._switch_controller_service(request) if response.ok: rospy.loginfo('Deactivated controller %s' % controller_name) else: rospy.loginfo('Deactivating controller %s failed' % controller_name) return response.ok return True
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 start_ros_controllers(): start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER, SwitchController) req = SwitchControllerRequest() # this two controller is same as the ur joint trajectory controller req.start_controllers.append("joint_state_controller") # req.start_controllers.append("my_cartesian_compliance_controller") # req.start_controllers.append("my_cartesian_force_controller") req.start_controllers.append("arm_controller") # req.start_controllers.append("my_cartesian_motion_controller") # req.start_controllers.append("my_motion_control_handle") req.strictness = req.BEST_EFFORT rospy.wait_for_service(SWITCH_CONTROLLER) res = start_controllers_client(req)
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 switch_to_position_controller(): rospy.wait_for_service("/controller_manager/list_controllers") rate = rospy.Rate(1) list_controllers = rospy.ServiceProxy( "/controller_manager/list_controllers", ListControllers) position_controller_available = False while not position_controller_available: res = list_controllers(ListControllersRequest()) position_controller_available = any([ controller.name == "joint_group_position_controller" for controller in res.controller ]) rate.sleep() rospy.logdebug("Switching controllers") rospy.wait_for_service("/controller_manager/switch_controller") switch_controller = rospy.ServiceProxy( '/controller_manager/switch_controller', SwitchController) req = SwitchControllerRequest() req.start_controllers = ["joint_group_position_controller"] req.stop_controllers = ["arm_controller"] req.strictness = 2 switch_controller(req)
rospy.init_node("test") joint_names = ['plat_joint','shoulder_joint','forearm_joint','wrist_joint'] starting_pos = np.array([0, 0, 1, 2]) model_config_proxy = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration) model_config_req = SetModelConfigurationRequest() model_config_req.model_name = 'arm' model_config_req.urdf_param_name = 'robot_description' model_config_req.joint_names = joint_names model_config_req.joint_positions = starting_pos controller_switch_proxy = rospy.ServiceProxy('/controller_manager/switch_controller',SwitchController) controller_switch_off = SwitchControllerRequest() controller_switch_off.stop_controllers = ['joint_state_controller','arm_controller'] controller_switch_off.strictness = 2 controller_switch_on = SwitchControllerRequest() controller_switch_on.start_controllers = ['joint_state_controller','arm_controller'] controller_switch_on.strictness = 2 rospy.wait_for_service('/controller_manager/switch_controller') try: controller_switch_proxy(controller_switch_off) except rospy.ServiceException, e: print('/controller_manager/switch_controller call failed') rospy.wait_for_service('/gazebo/set_model_configuration') try: model_config_proxy(model_config_req)