Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adapted rqt_jtc to newest control_msgs for jtc #643

Merged
merged 1 commit into from
Jun 4, 2023
Merged

Adapted rqt_jtc to newest control_msgs for jtc #643

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 13 additions & 13 deletions ...oint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
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