Skip to content

Commit

Permalink
Adapted rqt_jtc to newest control_msgs for jtc (#643)
Browse files Browse the repository at this point in the history
(cherry picked from commit b47b42d)
  • Loading branch information
gwalck authored and mergify[bot] committed Jun 11, 2023
1 parent f90c05b commit 9468a7e
Showing 1 changed file with 13 additions and 13 deletions.
Expand Up @@ -80,8 +80,8 @@ class JointTrajectoryController(Plugin):
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
- The controller exposes the C{command} and C{controller_state} topics
in its ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
Expand Down Expand Up @@ -252,10 +252,10 @@ def _update_jtc_list(self):
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()

def _on_joint_state_change(self, actual_pos):
assert len(actual_pos) == len(self._joint_pos)
for name in actual_pos.keys():
self._joint_pos[name]["position"] = actual_pos[name]
def _on_joint_state_change(self, current_pos):
assert len(current_pos) == len(self._joint_pos)
for name in current_pos.keys():
self._joint_pos[name]["position"] = current_pos[name]

def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
Expand Down Expand Up @@ -289,11 +289,11 @@ def _on_jtc_enabled(self, val):
self._speed_scaling_widget.setEnabled(val)

if val:
# Widgets send desired position commands to controller
# Widgets send reference position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
# Controller updates widgets with feedback position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()

Expand Down Expand Up @@ -332,7 +332,7 @@ def _load_jtc(self):

# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + "/state"
state_topic = jtc_ns + "/controller_state"
cmd_topic = jtc_ns + "/joint_trajectory"
self._state_sub = self._node.create_subscription(
JointTrajectoryControllerState, state_topic, self._state_cb, 1
Expand Down Expand Up @@ -404,12 +404,12 @@ def _unregister_executor(self):
self._executor = None

def _state_cb(self, msg):
actual_pos = {}
current_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
joint_pos = msg.feedback.positions[i]
current_pos[joint_name] = joint_pos
self.jointStateChanged.emit(current_pos)

def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]["command"] = val
Expand Down

0 comments on commit 9468a7e

Please sign in to comment.