diff --git a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controller.yaml
similarity index 97%
rename from panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml
rename to panda_gazebo/cfg/controllers/effort_joint_trajectory_controller.yaml
index eb73f201..ad5fa138 100644
--- a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml
+++ b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controller.yaml
@@ -2,7 +2,7 @@
# trajectory controllers that can control a joint effort hardware interface.
# NOTE: Equal to Franka's 'effort_joint_trajectory_controller' but with a different
# goal tolerance (see https://github.com/frankaemika/franka_ros/issues/201).
-panda_arm_controller:
+panda_arm_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints:
- panda_joint1
diff --git a/panda_gazebo/cfg/controllers/moveit_ros_control.yaml b/panda_gazebo/cfg/controllers/moveit_ros_control.yaml
index 00679ba3..2b4208a4 100644
--- a/panda_gazebo/cfg/controllers/moveit_ros_control.yaml
+++ b/panda_gazebo/cfg/controllers/moveit_ros_control.yaml
@@ -2,7 +2,7 @@
# Make controllers visible to MoveIt.
move_group:
controller_list:
- - name: panda_arm_controller
+ - name: panda_arm_trajectory_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
diff --git a/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml b/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml
index 39186669..66b798e2 100644
--- a/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml
+++ b/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml
@@ -2,7 +2,7 @@
# Make controllers visible to MoveIt.
move_group:
controller_list:
- - name: panda_arm_controller
+ - name: panda_arm_trajectory_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
diff --git a/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml
deleted file mode 100644
index ce25ef47..00000000
--- a/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-# Configuration file that contains the configuration values for setting up the panda
-# trajectory controllers.
-# NOTE: Equal to Franka's 'position_joint_trajectory_controller' but with a different
-# goal tolerance (see https://github.com/frankaemika/franka_ros/issues/201).
-panda_arm_controller:
- type: position_controllers/JointTrajectoryController
- joints:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- constraints:
- goal_time: 0.5
- panda_joint1: { goal: 0.005 }
- panda_joint2: { goal: 0.005 }
- panda_joint3: { goal: 0.005 }
- panda_joint4: { goal: 0.005 }
- panda_joint5: { goal: 0.005 }
- panda_joint6: { goal: 0.005 }
- panda_joint7: { goal: 0.005 }
diff --git a/panda_gazebo/launch/load_controllers.launch.xml b/panda_gazebo/launch/load_controllers.launch.xml
index 5be7d3ea..cb9684c8 100644
--- a/panda_gazebo/launch/load_controllers.launch.xml
+++ b/panda_gazebo/launch/load_controllers.launch.xml
@@ -18,21 +18,20 @@
-
-
-
-
-
-
-
+
+
+
+
+
-
-
-
+
+
+
+
+
-
-
+
+
+
diff --git a/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml b/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml
index 00691611..89b85ea1 100755
--- a/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml
+++ b/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml
@@ -1,12 +1,13 @@
-
+
-
+
+
-
+
-
-
-
+
+
+
diff --git a/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml b/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml
index 5cb14691..dfb5057c 100755
--- a/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml
+++ b/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml
@@ -1,12 +1,13 @@
-
+
-
+
+
-
-
-
+
+
+
diff --git a/panda_gazebo/launch/load_joint_group_position_controller.launch.xml b/panda_gazebo/launch/load_joint_group_position_controller.launch.xml
index fb1d4de5..88cb119e 100755
--- a/panda_gazebo/launch/load_joint_group_position_controller.launch.xml
+++ b/panda_gazebo/launch/load_joint_group_position_controller.launch.xml
@@ -1,12 +1,13 @@
-
+
-
+
+
-
-
-
+
+
+
diff --git a/panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml b/panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml
deleted file mode 100755
index 59eb8214..00000000
--- a/panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py
index 7bce8951..ba229385 100644
--- a/panda_gazebo/src/panda_gazebo/core/control_server.py
+++ b/panda_gazebo/src/panda_gazebo/core/control_server.py
@@ -87,7 +87,7 @@
}
ARM_POSITION_CONTROLLER = "panda_arm_joint_position_controller"
ARM_EFFORT_CONTROLLER = "panda_arm_joint_effort_controller"
-ARM_TRAJ_CONTROLLER = "panda_arm_controller"
+ARM_TRAJ_CONTROLLER = "panda_arm_trajectory_controller"
HAND_CONTROLLER = "franka_gripper"
CONTROLLER_INFO_RATE = 1 / 10 # Rate [hz] for retrieving controller information.
CONNECTION_TIMEOUT = 10 # Service connection timeout [s].
@@ -325,23 +325,26 @@ def __init__( # noqa: C901
# servers ##############################
########################################
# NOTE: Here setup a new action service that serves as a wrapper around the
- # original 'panda_arm_controller/follow_joint_trajectory'. By doing this
- # we add the following features to the original action servers.
+ # original 'panda_arm_trajectory_controller/follow_joint_trajectory'. By doing
+ # this we add the following features to the original action servers.
# - The ability to send partial joint messages.
# - The ability to send joint trajectory messages that do not specify joints.
# - The ability to automatic generate a time axes when the create_time_axis
# field is set to True.
if load_arm_follow_joint_trajectory_action:
- # Connect to original 'panda_arm_controller/follow_joint_trajectory' action
- # server.
+ # Connect to the 'panda_arm_trajectory_controller/follow_joint_trajectory'
+ # action server.
rospy.logdebug(
- "Connecting to 'panda_arm_controller/follow_joint_trajectory' action "
- "service."
+ "Connecting to '{}' action service.".format(
+ "panda_arm_trajectory_controller/follow_joint_trajectory"
+ )
)
- if action_server_exists("panda_arm_controller/follow_joint_trajectory"):
+ if action_server_exists(
+ "panda_arm_trajectory_controller/follow_joint_trajectory"
+ ):
# Connect to robot control action server.
self._arm_joint_traj_client = SimpleActionClient(
- "panda_arm_controller/follow_joint_trajectory",
+ "panda_arm_trajectory_controller/follow_joint_trajectory",
control_msgs.FollowJointTrajectoryAction,
)
@@ -352,18 +355,18 @@ def __init__( # noqa: C901
if not retval:
rospy.logwarn(
"No connection could be established with the "
- "'panda_arm_controller/follow_joint_trajectory' service. The "
- "Panda Robot Environment therefore can not use this action "
- "service to control the Panda Robot."
+ "'panda_arm_trajectory_controller/follow_joint_trajectory' "
+ "service. The Panda Robot Environment therefore can not use "
+ "this action service to control the Panda Robot."
)
else:
self._arm_joint_traj_client_connected = True
else:
rospy.logwarn(
"No connection could be established with the "
- "'panda_arm_controller/follow_joint_trajectory' service. The Panda "
- "Robot Environment therefore can not use this action service to "
- "control the Panda Robot."
+ "'panda_arm_trajectory_controller/follow_joint_trajectory' "
+ "service. The Panda Robot Environment therefore can not use this "
+ "action service to control the Panda Robot."
)
# Setup a new Panda arm joint trajectory action server.
@@ -489,7 +492,7 @@ def _wait_till_arm_control_done(
def _create_arm_traj_action_server_msg(self, input_msg): # noqa: C901
"""Converts the ``control_msgs.msg.FollowJointTrajectoryGoal`` message that
is received by the ``panda_control_server`` follow joint trajectory wrapper
- action servers into the right format for the original ``panda_arm_controller``
+ action servers into the right format for the original ``panda_arm_trajectory_controller``
`follow_joint_trajectory `_
action server.
@@ -503,13 +506,13 @@ def _create_arm_traj_action_server_msg(self, input_msg): # noqa: C901
message we want to validate.
Returns:
- dict: A dictionary containing the arm and hand panda_arm_controller
+ dict: A dictionary containing the arm and hand panda_arm_trajectory_controller
:control_msgs:`control_msgs.msg.FollowJointTrajectoryGoal `
messages.
Raises:
:obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when the
- input_msg could not be converted into panda_arm_controller control
+ input_msg could not be converted into panda_arm_trajectory_controller control
messages.
""" # noqa: E501
input_joint_names = input_msg.trajectory.joint_names
@@ -1789,7 +1792,7 @@ def _arm_joint_traj_execute_cb(self, goal):
def _arm_joint_traj_feedback_cb(self, feedback):
"""Relays the feedback messages from the original
- ``panda_arm_controller/follow_joint_trajectory`` server to our to our
+ ``panda_arm_trajectory_controller/follow_joint_trajectory`` server to our to our
``panda_control_server/panda_arm/follow_joint_trajectory`` wrapper action
server.
@@ -1802,9 +1805,10 @@ def _arm_joint_traj_feedback_cb(self, feedback):
def _arm_joint_traj_preempt_cb(self):
"""Relays the preempt request made to the
``panda_control_server/panda_arm/follow_joint_trajectory`` action server wrapper
- to the original ``panda_arm_controller/follow_joint_trajectory`` action server.
+ to the original ``panda_arm_trajectory_controller/follow_joint_trajectory``
+ action server.
"""
- # Stop panda_arm_controller action server.
+ # Stop panda_arm_trajectory_controller action server.
if self._arm_joint_traj_client.get_state() in [
GoalStatus.PREEMPTING,
GoalStatus.ACTIVE,
diff --git a/panda_gazebo/src/panda_gazebo/core/control_switcher.py b/panda_gazebo/src/panda_gazebo/core/control_switcher.py
index a0dbdab3..375723b9 100644
--- a/panda_gazebo/src/panda_gazebo/core/control_switcher.py
+++ b/panda_gazebo/src/panda_gazebo/core/control_switcher.py
@@ -1,6 +1,7 @@
"""This class is responsible for switching the control type that is used for
-controlling the Panda Robot robot ``arm``. It serves as a wrapper around the services
-created by the ROS `controller_manager `_ class.
+controlling the Panda Robot robot arm and hand. It serves as a wrapper around the
+services created by the ROS
+`controller_manager `_ class.
Control types:
* `trajectory `_
@@ -31,37 +32,20 @@
# Global script vars.
ARM_CONTROLLERS = {
- "end_effector": "panda_arm_controller",
- "trajectory": "panda_arm_controller",
- "position": [
- "panda_arm_joint1_position_controller",
- "panda_arm_joint2_position_controller",
- "panda_arm_joint3_position_controller",
- "panda_arm_joint4_position_controller",
- "panda_arm_joint5_position_controller",
- "panda_arm_joint6_position_controller",
- "panda_arm_joint7_position_controller",
- ],
- "effort": [
- "panda_arm_joint1_effort_controller",
- "panda_arm_joint2_effort_controller",
- "panda_arm_joint3_effort_controller",
- "panda_arm_joint4_effort_controller",
- "panda_arm_joint5_effort_controller",
- "panda_arm_joint6_effort_controller",
- "panda_arm_joint7_effort_controller",
- ],
+ "end_effector": "panda_arm_trajectory_controller",
+ "trajectory": "panda_arm_trajectory_controller",
+ "position": "panda_arm_joint_position_controller",
+ "effort": "panda_arm_joint_effort_controller",
}
HAND_CONTROLLERS = {
- "position": [
- "franka_gripper",
- ],
+ "position": "franka_gripper",
}
CONTROLLER_DICT = {"arm": ARM_CONTROLLERS, "hand": HAND_CONTROLLERS}
class ControllerSwitcherResponse:
- """Class used for returning the result of the ControllerSwitcher.switch method.
+ """Class used for returning the result of the :meth:`PandaControlSwitcher.switch`
+ method.
Attributes:
success (bool): Specifies whether the switch operation was successful.
@@ -69,7 +53,7 @@ class ControllerSwitcherResponse:
"""
def __init__(self, success=True, prev_control_type=""):
- """Initialise ControllerSwitcher response object.
+ """Initialise ControllerSwitcherResponse object.
Args:
success (bool, optional): Whether the switch operation was successful.
@@ -85,8 +69,10 @@ class PandaControlSwitcher(object):
"""Used for switching the Panda robot controllers.
Attributes:
- verbose : bool
- Boolean specifying whether we want to display log messages during switching.
+ verbose (bool): Boolean specifying whether we want to display log messages
+ during switching.
+ arm_control_types (list): List of currently active arm control types.
+ hand_control_types (list): List of currently active hand control types.
"""
def __init__(self, connection_timeout=10, verbose=True, robot_name_space=""):
@@ -101,7 +87,6 @@ def __init__(self, connection_timeout=10, verbose=True, robot_name_space=""):
'controller_manager' is on. Defaults to ``""``.
"""
self.verbose = verbose
- self._controller_manager_response_timeout = 3
self._controller_spawner_wait_timeout = 5
# Connect to controller_manager services.
@@ -158,8 +143,8 @@ def _list_controllers_state(self): # noqa: C901
Returns:
dict: Dictionary containing information about which controllers are
- currently running or initialised divided by control group
- arm/hand and other).
+ currently running or loaded divided by control group arm/hand and
+ other).
"""
list_controllers_resp = self._list_controller_client.call(
ListControllersRequest()
@@ -240,39 +225,31 @@ def _load(self, controllers):
return [False]
@property
- def arm_control_type(self):
- """Returns the currently active arm control type. Returns empty string when no
+ def arm_control_types(self):
+ """Returns the currently active arm control types. Returns empty list when no
control type is enabled.
"""
- try:
- arm_control_type = list(
- self._list_controllers_state()["arm"]["running"].keys()
- )[0]
- except KeyError:
- arm_control_type = ""
- return arm_control_type
+ controllers_state = self._list_controllers_state()
+ arm_state = controllers_state.get("arm", {})
+ return list(arm_state.get("running", {}).keys())
@property
- def hand_control_type(self):
- """Returns the currently active hand control type. Returns empty string when no
+ def hand_control_types(self):
+ """Returns the currently active hand control types. Returns empty when no
control type is enabled.
"""
- try:
- hand_control_type = list(
- self._list_controllers_state()["hand"]["running"].keys()
- )[0]
- except KeyError:
- hand_control_type = ""
- return hand_control_type
+ controllers_state = self._list_controllers_state()
+ hand_state = controllers_state.get("hand", {})
+ return list(hand_state.get("running", {}).keys())
def wait_for_control_type(self, control_group, control_type, timeout=None, rate=10):
"""Function that can be used to wait till all the controllers used for a given
- 'control_group' and 'control_type' are running. Useful 6 when you expect a
+ 'control_group' and 'control_type' are running. Useful four when you expect a
launch file to load certain controllers.
Args:
control_group (str): The control group of which you want the switch the
- control type. Options are 'hand' or 'arm'.
+ control type. Options are ``hand`` or ``arm``.
control_type (str): The robot control type you want to switch to for the
given 'control_group'. Options are: ``trajectory``, ``position`` and
``effort``.
@@ -285,17 +262,16 @@ def wait_for_control_type(self, control_group, control_type, timeout=None, rate=
TimeoutError: Thrown when the set timeout has passed.
"""
start_time = time.time()
- timeout = timeout if timeout else -1
+ timeout = timeout if timeout else float("inf")
while (
- (control_type == "arm" and control_type != self.arm_control_type)
- or (control_type == "hand" and control_type != self.hand_control_type)
- and time.time() - start_time <= timeout
- ):
+ (control_group == "arm" and control_type not in self.arm_control_types)
+ or (control_group == "hand" and control_type not in self.hand_control_types)
+ ) and time.time() - start_time <= timeout:
time.sleep(1.0 / rate)
if time.time() - start_time > timeout:
raise TimeoutError(
f"Control type '{control_type}' for control_group '{control_group}' "
- "was not spawned within the set timeout (i.e. {timeout})."
+ f"was not spawned within the set timeout (i.e. {timeout})."
)
def switch( # noqa: C901
@@ -303,7 +279,7 @@ def switch( # noqa: C901
control_group,
control_type,
load_controllers=True,
- timeout=None,
+ timeout=3,
verbose=None,
):
"""Switch Panda robot control type. This function stops all currently running
@@ -311,14 +287,14 @@ def switch( # noqa: C901
Args:
control_group (str): The control group of which you want the switch the
- control type. Options are 'hand' or 'arm'.
+ control type. Options are ``hand`` or ``arm``.
control_type (str): The robot control type you want to switch to for the
given 'control_group'. Options are: ``trajectory``, ``position``
and ``effort``.
load_controllers (bool): Try to load the required controllers for a given
control_type if they are not yet loaded.
- timeout (int, optional): The timout for switching to a given controller.
- Defaults to :attr:`self._controller_manager_response_timeout`.
+ timeout (int, optional): The timeout for switching to a given controller.
+ Defaults to ``3`` sec.
verbose (bool, optional): Whether to display debug log messages. Defaults
to verbose value set during the class initiation.
@@ -328,8 +304,7 @@ def switch( # noqa: C901
'success' and the previously used controller 'prev_control_type'.
"""
resp = ControllerSwitcherResponse()
- if not verbose:
- verbose = self.verbose
+ verbose = self.verbose if verbose is None else verbose
# Validate input arguments.
control_type = control_type.lower()
@@ -344,78 +319,63 @@ def switch( # noqa: C901
return resp
else:
control_group = control_group[0]
- if control_group not in CONTROLLER_DICT.keys():
+ if control_group not in CONTROLLER_DICT:
rospy.logwarn(
f"The '{control_group}' control group you specified is not valid. "
"Valid control groups for the Panda robot are "
- f"{CONTROLLER_DICT.keys()}."
+ f"{list(CONTROLLER_DICT.keys())}."
)
resp.success = False
return resp
- if control_type not in CONTROLLER_DICT[control_group].keys():
+ if control_type not in CONTROLLER_DICT[control_group]:
rospy.logwarn(
f"The '{control_type} control type you specified is not valid. Valid "
"control types for the Panda robot are "
- f"{CONTROLLER_DICT[control_group].keys()}."
+ f"{list(CONTROLLER_DICT[control_group].keys())}."
)
resp.success = False
return resp
- if not timeout:
- switch_timeout = self._controller_manager_response_timeout
- else:
- switch_timeout = timeout
- # Get active controllers.
- # NOTE: Here we wait a bit till we are sure the controller_spawner is ready
+ # Retrieve information about the currently running and loaded controllers.
+ # NOTE: Here we wait a bit till we are sure the controller_spawner is ready.
start_time = time.time()
controllers_state = {}
while (
control_group not in controllers_state
- or "running" not in controllers_state[control_group].keys()
+ or "running" not in controllers_state[control_group]
) and time.time() - start_time <= self._controller_spawner_wait_timeout:
controllers_state = self._list_controllers_state()
-
- # Generate switch controller msg.
- prev_control_type = (
- str(list(controllers_state[control_group]["running"].keys())[0])
- if (
- control_group in controllers_state
- and "running" in controllers_state[control_group].keys()
- )
- else ""
- )
- controller_already_running = False
+ running_controllers = controllers_state.get(control_group, {}).get("running")
+ prev_control_type = ", ".join(running_controllers.keys())
+ running_state = controllers_state.get(control_group, {}).get("running", {})
+ running_control_types = list(running_state.keys())
+ running_controllers = get_unique_list(flatten_list(running_state.values()))
+ stopped_state = controllers_state.get(control_group, {}).get("stopped", {})
+ stopped_control_types = list(stopped_state.keys())
+ loaded_state = controllers_state.get(control_group, {}).get("loaded", {})
+ loaded_control_types = list(loaded_state.keys())
+ controllers = CONTROLLER_DICT[control_group][control_type]
+
+ # Generate switch controller msg and load controllers if needed.
switch_controller_msg = SwitchControllerRequest()
switch_controller_msg.strictness = SwitchControllerRequest.STRICT
- switch_controller_msg.timeout = switch_timeout
- try:
- running_control_types = controllers_state[control_group]["running"].keys()
- running_controllers = get_unique_list(
- flatten_list(controllers_state[control_group]["running"].values())
- )
- except KeyError:
- running_control_types, running_controllers = [], []
- try:
- stopped_control_types = controllers_state[control_group]["stopped"].keys()
- except KeyError:
- stopped_control_types = []
- try:
- loaded_control_types = controllers_state[control_group]["loaded"].keys()
- except KeyError:
- loaded_control_types = []
+ switch_controller_msg.timeout = timeout
+ switch_controller_msg.start_controllers = (
+ controllers if isinstance(controllers, list) else [controllers]
+ )
if (
control_type in running_control_types
): # If control type controllers are already running.
- controller_already_running = True
+ if verbose:
+ rospy.logdebug(
+ f"Panda {control_group} control type not switched to "
+ f"'{control_type}' as the Panda robot was already using "
+ f"'{prev_control_type}'."
+ )
+ resp.success = True
+ resp.prev_control_type = prev_control_type
+ return resp
elif control_type in stopped_control_types: # If controller was stopped.
- # Fill the start_controllers field of the switch control message.
- switch_controller_msg.start_controllers = (
- CONTROLLER_DICT[control_group][control_type]
- if isinstance(CONTROLLER_DICT[control_group][control_type], list)
- else [CONTROLLER_DICT[control_group][control_type]]
- )
-
- # Fill the stop_controllers field of the switch control message.
switch_controller_msg.stop_controllers = flatten_list(
controllers_state[control_group]["running"].values()
)
@@ -424,10 +384,10 @@ def switch( # noqa: C901
): # Try to load the controllers if not yet loaded.
# Load the required controllers.
if load_controllers:
- retval = self._load(CONTROLLER_DICT[control_group][control_type])
+ retval = self._load(controllers)
failed_controllers = list(
compress(
- CONTROLLER_DICT[control_group][control_type],
+ controllers,
[not val for val in retval],
)
)
@@ -435,7 +395,7 @@ def switch( # noqa: C901
rospy.logwarn(
f"The Panda {control_group} control group was not switched to "
f"'{control_type}' because the "
- f"{CONTROLLER_DICT[control_group][control_type]} controllers could "
+ f"{controllers} controllers could "
"not be loaded as 'load_controllers' was set to argument was set "
"to 'False'."
)
@@ -445,21 +405,14 @@ def switch( # noqa: C901
# Check if all controllers were loaded successfully.
if len(failed_controllers) == 0:
- # Fill the start_controllers field of the switch control message.
- switch_controller_msg.start_controllers = (
- CONTROLLER_DICT[control_group][control_type]
- if isinstance(CONTROLLER_DICT[control_group][control_type], list)
- else [CONTROLLER_DICT[control_group][control_type]]
- )
-
- # Fill the stop_controllers field of the switch control message.
switch_controller_msg.stop_controllers = running_controllers
else:
if (
- control_group == "arm" and control_type is not self.arm_control_type
+ control_group == "arm"
+ and control_type not in self.arm_control_types
) or (
control_group == "hand"
- and control_type is not self.hand_control_type
+ and control_type not in self.hand_control_types
):
rospy.logwarn(
f"The Panda {control_group} control griyo was not switched to "
@@ -472,53 +425,30 @@ def switch( # noqa: C901
resp.prev_control_type = prev_control_type
return resp
else:
- # Fill the start_controllers field of the switch control message.
- switch_controller_msg.start_controllers = (
- CONTROLLER_DICT[control_group][control_type]
- if isinstance(CONTROLLER_DICT[control_group][control_type], list)
- else [CONTROLLER_DICT[control_group][control_type]]
- )
-
- # Fill the stop_controllers field of the switch control message.
switch_controller_msg.stop_controllers = running_controllers
# Send switch_controller msgs.
- if not controller_already_running:
+ rospy.logdebug(
+ f"Switching Panda {control_group} control group to '{control_type}'."
+ )
+ retval = self._switch_controller_client(switch_controller_msg)
+ if retval.ok:
rospy.logdebug(
- f"Switching Panda {control_group} control group to '{control_type}'."
+ f"Switching Panda {control_group} control group to "
+ f"'{control_type}' successful."
)
- retval = self._switch_controller_client(switch_controller_msg)
- if retval.ok:
- rospy.logdebug(
- f"Switching Panda {control_group} control group to "
- f"'{control_type}' successful."
- )
- resp.success = retval.ok
- resp.prev_control_type = prev_control_type
- return resp
- else:
- if (
- control_group == "arm" and control_type is not self.arm_control_type
- ) or (
- control_group == "hand"
- and control_type is not self.arm_control_type
- ):
- rospy.logwarn(
- f"Switching Panda {control_group} control type to "
- f"'{control_type}' failed."
- )
- resp.success = retval.ok
- else:
- resp.success = True
- resp.prev_control_type = prev_control_type
- return resp
else:
- if verbose:
- rospy.logdebug(
- f"Panda {control_group} control type not switched to "
- "'{control_type}' as the Panda robot was already using "
- f"'{prev_control_type}'."
+ if (
+ control_group == "arm" and control_type not in self.arm_control_types
+ ) or (
+ control_group == "hand" and control_type not in self.arm_control_types
+ ):
+ rospy.logwarn(
+ f"Switching Panda {control_group} control type to "
+ f"'{control_type}' failed."
)
- resp.success = True
- resp.prev_control_type = prev_control_type
- return resp
+ else:
+ retval.ok = True
+ resp.success = retval.ok
+ resp.prev_control_type = prev_control_type
+ return resp
diff --git a/panda_gazebo/tests/manual/panda_control_switcher_test.py b/panda_gazebo/tests/manual/panda_control_switcher_test.py
index 0e819b95..23f0147c 100644
--- a/panda_gazebo/tests/manual/panda_control_switcher_test.py
+++ b/panda_gazebo/tests/manual/panda_control_switcher_test.py
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
-"""Smalls script to test if the ControlSwitcher class is working.
-"""
+"""Smalls script to test if the ControlSwitcher class is working."""
from panda_gazebo.core.control_switcher import PandaControlSwitcher
if __name__ == "__main__":
@@ -11,8 +10,17 @@
arm_switch_resp = control_switcher.switch(
control_group="arm", control_type="effort"
)
+ control_switcher.wait_for_control_type(control_group="arm", control_type="effort")
hand_switch_resp = control_switcher.switch(
- control_group="hand", control_type="effort"
+ control_group="hand", control_type="position"
+ ) # NOTE: Currently the hand only supports position control.
+
+ # Switch controllers back.
+ arm_switch_resp = control_switcher.switch(
+ control_group="arm", control_type="trajectory"
)
+ hand_switch_resp = control_switcher.switch(
+ control_group="hand", control_type="position"
+ ) # NOTE: Currently the hand only supports position control.
print("You can put your debug point here")