Skip to content

Commit

Permalink
find limits of jtc joints | added robot_description combo
Browse files Browse the repository at this point in the history 
Signed-off-by: Jakub Delicat <[email protected]>
  • Loading branch information
@delihus
delihus committed May 10, 2024
1 parent fe07b83 commit 40d7516
Show file tree
Hide file tree
Showing 3 changed files with 131 additions and 57 deletions.
24 changes: 19 additions & 5 deletions rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>336</width>
<height>317</height>
<width>616</width>
<height>594</height>
</rect>
</property>
<property name="windowTitle">
Expand All @@ -26,8 +26,11 @@
<widget class="QComboBox" name="jtc_combo"/>
</item>
<item row="1" column="0">
<widget class="QComboBox" name="cm_combo"/>
<widget class="QComboBox" name="cm_combo"/>
</item>
<item row="1" column="2">
<widget class="QComboBox" name="robot_description_combo"/>
</item>
<item row="0" column="0">
<widget class="QLabel" name="cm_list_label">
<property name="toolTip">
Expand All @@ -51,6 +54,16 @@
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="robot_description_list_label">
<property name="text">
<string>robot description topic</string>
</property>
<property name="buddy">
<cstring>robot_description_combo</cstring>
</property>
</widget>
</item>
</layout>
</item>
<item>
Expand Down Expand Up @@ -172,8 +185,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>314</width>
<height>109</height>
<width>566</width>
<height>300</height>
</rect>
</property>
</widget>
Expand All @@ -195,6 +208,7 @@
<tabstops>
<tabstop>cm_combo</tabstop>
<tabstop>jtc_combo</tabstop>
<tabstop>robot_description_combo</tabstop>
<tabstop>enable_button</tabstop>
</tabstops>
<resources/>
Expand Down
119 changes: 71 additions & 48 deletions rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,26 +27,47 @@
from math import pi

import rclpy
import rclpy.subscription
from std_msgs.msg import String

description = ""
robot_description_subscriber_created = False
subscription = None


def callback(msg):
global description
description = msg.data


def subscribe_to_robot_description(node, key="robot_description"):
def subscribe_to_robot_description(
node, key="robot_description"
) -> rclpy.subscription.Subscription:
global robot_description_subscriber_created
global subscription
qos_profile = rclpy.qos.QoSProfile(depth=1)
qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL
qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE

node.create_subscription(String, key, callback, qos_profile)
subscription = node.create_subscription(String, key, callback, qos_profile)
robot_description_subscriber_created = True
return subscription


def get_joint_limits(node, use_smallest_joint_limits=True):
def unsubscribe_to_robot_description(node) -> rclpy.subscription.Subscription:
global subscription
if subscription is not None:
node.destroy_subscription(subscription)


def get_joint_limits(node, joints_names, use_smallest_joint_limits=True):
global robot_description_subscriber_created
global description

if not robot_description_subscriber_created:
print("First select robot description topic name!")
return

use_small = use_smallest_joint_limits
use_mimic = True

Expand All @@ -71,54 +92,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True):
if jtype == "fixed":
continue
name = child.getAttribute("name")
try:
limit = child.getElementsByTagName("limit")[0]

if name in joints_names:
try:
minval = float(limit.getAttribute("lower"))
maxval = float(limit.getAttribute("upper"))
except ValueError:
if jtype == "continuous":
minval = -pi
maxval = pi
else:
limit = child.getElementsByTagName("limit")[0]
try:
minval = float(limit.getAttribute("lower"))
maxval = float(limit.getAttribute("upper"))
except ValueError:
if jtype == "continuous":
minval = -pi
maxval = pi
else:
raise Exception(
f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
)
try:
maxvel = float(limit.getAttribute("velocity"))
except ValueError:
raise Exception(
f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
)
try:
maxvel = float(limit.getAttribute("velocity"))
except ValueError:
except IndexError:
raise Exception(
f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
f"Missing limits tag for the joint : {name} in the robot_description!"
)
except IndexError:
raise Exception(
f"Missing limits tag for the joint : {name} in the robot_description!"
)
safety_tags = child.getElementsByTagName("safety_controller")
if use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute("soft_lower_limit"):
minval = max(minval, float(tag.getAttribute("soft_lower_limit")))
if tag.hasAttribute("soft_upper_limit"):
maxval = min(maxval, float(tag.getAttribute("soft_upper_limit")))

mimic_tags = child.getElementsByTagName("mimic")
if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {"parent": tag.getAttribute("joint")}
if tag.hasAttribute("multiplier"):
entry["factor"] = float(tag.getAttribute("multiplier"))
if tag.hasAttribute("offset"):
entry["offset"] = float(tag.getAttribute("offset"))

dependent_joints[name] = entry
continue

if name in dependent_joints:
continue

joint = {"min_position": minval, "max_position": maxval}
joint["has_position_limits"] = jtype != "continuous"
joint["max_velocity"] = maxvel
free_joints[name] = joint
safety_tags = child.getElementsByTagName("safety_controller")
if use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute("soft_lower_limit"):
minval = max(minval, float(tag.getAttribute("soft_lower_limit")))
if tag.hasAttribute("soft_upper_limit"):
maxval = min(maxval, float(tag.getAttribute("soft_upper_limit")))

mimic_tags = child.getElementsByTagName("mimic")
if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {"parent": tag.getAttribute("joint")}
if tag.hasAttribute("multiplier"):
entry["factor"] = float(tag.getAttribute("multiplier"))
if tag.hasAttribute("offset"):
entry["offset"] = float(tag.getAttribute("offset"))

dependent_joints[name] = entry
continue

if name in dependent_joints:
continue

joint = {"min_position": minval, "max_position": maxval}
joint["has_position_limits"] = jtype != "continuous"
joint["max_velocity"] = maxvel
free_joints[name] = joint
return free_joints
45 changes: 41 additions & 4 deletions ...oint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,11 @@

from .utils import ControllerLister, ControllerManagerLister
from .double_editor import DoubleEditor
from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description
from .joint_limits_urdf import (
get_joint_limits,
subscribe_to_robot_description,
unsubscribe_to_robot_description,
)
from .update_combo import update_combo

# TODO:
Expand Down Expand Up @@ -170,19 +174,30 @@ def __init__(self, context):
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()

# subscriptions
subscribe_to_robot_description(self._node)
# Timer for running controller updates
self._update_robot_description_list_timer = QTimer(self)
self._update_robot_description_list_timer.setInterval(
int(1000.0 / self._ctrlrs_update_freq)
)
self._update_robot_description_list_timer.timeout.connect(
self._update_robot_description_list
)
self._update_robot_description_list_timer.start()

# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
w.robot_description_combo.currentIndexChanged[str].connect(
self._on_robot_description_change
)

self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber

self._list_controllers = None
self._list_robot_descriptions = None

def shutdown_plugin(self):
self._update_cmd_timer.stop()
Expand Down Expand Up @@ -238,7 +253,10 @@ def _update_jtc_list(self):
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation
for jtc_info in running_jtc:
self._robot_joint_limits = get_joint_limits(
self._node, _jtc_joint_names(jtc_info)
) # Lazy evaluation
valid_jtc = []
if self._robot_joint_limits:
for jtc_info in running_jtc:
Expand All @@ -252,6 +270,18 @@ def _update_jtc_list(self):
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))

def _update_robot_description_list(self):
if not self._list_robot_descriptions:
self._widget.robot_description_combo.clear()
self._list_robot_descriptions = []

topics_with_types = self._node.get_topic_names_and_types()
for topic_with_type in topics_with_types:
if "std_msgs/msg/String" in topic_with_type[1]:
self._list_robot_descriptions.append(topic_with_type[0])

update_combo(self._widget.robot_description_combo, sorted(self._list_robot_descriptions))

def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()

Expand All @@ -272,6 +302,13 @@ def _on_cm_change(self, cm_ns):
else:
self._list_controllers = None

def _on_robot_description_change(self, robot_description):
unsubscribe_to_robot_description(self._node)

subscribe_to_robot_description(self._node, robot_description)
self._widget.jtc_combo.clear()
self._update_jtc_list()

def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
Expand Down

0 comments on commit 40d7516

Please sign in to comment.