def list_controller_types():
rclpy.wait_for_service('controller_manager/list_controller_types')
s = rospy.ServiceProxy('controller_manager/list_controller_types',
ListControllerTypes)
resp = s.call(ListControllerTypesRequest())
for t in resp.types:
print(t)
def unload_controller(name):
rclpy.wait_for_service('controller_manager/unload_controller')
s = rospy.ServiceProxy('controller_manager/unload_controller',
UnloadController)
resp = s.call(UnloadControllerRequest(name))
if resp.ok == 1:
print("Unloaded \'" + name + "\' successfully")
return True
else:
print("Error when unloading \'" + name + "\'")
return False
def load_controller(name):
rclpy.wait_for_service('controller_manager/load_controller')
s = rospy.ServiceProxy('controller_manager/load_controller',
LoadController)
resp = s.call(LoadControllerRequest(name))
if resp.ok:
print("Loaded \'" + name + "\'")
return True
else:
print("Error when loading \'" + name + "\'")
return False
def list_controllers():
rclpy.wait_for_service('controller_manager/list_controllers')
s = rclpy.ServiceProxy('controller_manager/list_controllers',
ListControllers)
resp = s.call(ListControllersRequest())
if len(resp.controller) == 0:
print("No controllers are loaded in mechanism control")
else:
for c in resp.controller:
hwi = list(set(r.hardware_interface for r in c.claimed_resources))
print("'%s' - '%s' ( %s )" % (c.name, "+".join(hwi), c.state))
def start_stop_controllers(start_controllers=[], stop_controllers=[]):
rclpy.wait_for_service('controller_manager/switch_controller')
s = rclpy.ServiceProxy('controller_manager/switch_controller',
SwitchController)
strictness = SwitchControllerRequest.STRICT
resp = s.call(
SwitchControllerRequest(start_controllers, stop_controllers,
strictness))
if resp.ok == 1:
if start_controllers:
print("Started {} successfully".format(start_controllers))
if stop_controllers:
print("Stopped {} successfully".format(stop_controllers))
return True
else:
print("Error when starting {} and stopping {}".format(
start_controllers, stop_controllers))
return False
def reload_libraries(force_kill, restore=False):
rclpy.wait_for_service('controller_manager/reload_controller_libraries')
s = rclpy.ServiceProxy('controller_manager/reload_controller_libraries',
ReloadControllerLibraries)
list_srv = rospy.ServiceProxy('controller_manager/list_controllers',
ListControllers)
load_srv = rospy.ServiceProxy('controller_manager/load_controller',
LoadController)
switch_srv = rospy.ServiceProxy('controller_manager/switch_controller',
SwitchController)
print("Restore: " + str(restore))
if restore:
originally = list_srv.call(ListControllersRequest())
resp = s.call(ReloadControllerLibrariesRequest(force_kill))
if resp.ok:
print("Successfully reloaded libraries")
result = True
else:
print(
"Failed to reload libraries. Do you still have controllers loaded?"
)
result = False
if restore:
for c in originally.controller:
load_srv(c.name)
to_start = [
c.name for c in originally.controller if c.state == 'running'
]
switch_srv(start_controllers=to_start,
stop_controllers=[],
strictness=SwitchControllerRequest.BEST_EFFORT)
print("Controllers restored to original state")
return result
def __init__(self):
self.config = self.node.get_parameter("behavior/head")
self.head_capsule = HeadCapsule(self)
self.world_model = WorldModelCapsule(self)
rclpy.wait_for_service('bio_ik/get_bio_ik') # geht das so?
self.bio_ik = self.create_client(GetIK, 'bio_ik/get_bio_ik')
request.group_name = "Head"
request.timeout.secs = 1
request.approximate = True
request.look_at_goals.append(LookAtGoal())
request.look_at_goals[0].link_name = camera_frame
request.look_at_goals[0].weight = 1
request.look_at_goals[0].axis.x = 1
pos_msg = JointCommand()
pos_msg.joint_names = ["HeadPan", "HeadTilt"]
pos_msg.positions = [0, 0]
pos_msg.velocities = [1.5, 1.5]
pos_msg.accelerations = [-1, -1]
pos_msg.max_currents = [-1, -1]
rclpy.wait_for_service("bio_ik/get_bio_ik")
bio_ik = node.create_client(GetIK, 'bio_ik/get_bio_ik')
publish_motor_goals = node.create_publisher(JointCommand,
'head_motor_goals', 10)
while rclpy.ok():
x = float(input('x: '))
y = float(input('y: '))
point = PointStamped()
point.header.stamp = node.get_clock().now()
point.header.frame_id = base_footprint_frame
point.point.x = x
point.point.y = y
point.point.z = 0
