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 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 _activate_controller(self, controller_name):
if not self._is_controller_running(controller_name):
request = SwitchControllerRequest()
request.start_controllers = [controller_name]
request.strictness = request.STRICT
response = self._switch_controller_service(request)
if response.ok:
rospy.loginfo('Activated controller %s' % controller_name)
else:
rospy.loginfo('Activating 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 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)
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)
print("REEEEEEEEEEEEE")
except rospy.ServiceException, e:
print('/gazebo/set_model_configuration call failed')
