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.

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

Fix formatting because pre-commit was not running on CI for some time. #409

Merged
merged 3 commits into from
Aug 15, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -139,5 +139,5 @@ repos:
rev: v2.1.0
hooks:
- id: codespell
args: ['--write-changes']
args: ['--write-changes', '--uri-ignore-words-list=ist']
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,11 @@
#ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_

#include <rclcpp/rclcpp.hpp>
#include <string>
#include <unordered_map>

#include "rclcpp/rclcpp.hpp"

namespace joint_trajectory_controller
{
static const rclcpp::Logger LOGGER =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ class DoubleEditor(QWidget):
# DoubleEditor?
"""
Widget that allows to edit the value of a floating-point value.

Optionally subject to lower and upper bounds.
"""

Expand All @@ -59,15 +60,17 @@ def __init__(self, min_val, max_val):
self._max_val = max_val

# Load editor UI
ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'),
'resource', 'double_editor.ui')
ui_file = os.path.join(
get_package_share_directory("rqt_joint_trajectory_controller"),
"resource",
"double_editor.ui",
)
loadUi(ui_file, self)

# Setup widget ranges and slider scale factor
self.slider.setRange(0, 100)
self.slider.setSingleStep(1)
self._scale = (max_val - min_val) / \
(self.slider.maximum() - self.slider.minimum())
self._scale = (max_val - min_val) / (self.slider.maximum() - self.slider.minimum())
self.spin_box.setRange(min_val, max_val)
self.spin_box.setSingleStep(self._scale)

Expand All @@ -82,8 +85,7 @@ def _slider_to_val(self, sval):
return self._min_val + self._scale * (sval - self.slider.minimum())

def _val_to_slider(self, val):
return round(self.slider.minimum() + (val - self._min_val) /
self._scale)
return round(self.slider.minimum() + (val - self._min_val) / self._scale)

def _on_slider_changed(self):
val = self._slider_to_val(self.slider.value())
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,19 @@
#!/usr/bin/env python

# Copyright 2022 PAL Robotics S.L.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got
# Exception: Required attribute not set in XML: upper
# upper is an optional attribute, so I don't understand what's going on
Expand All @@ -19,7 +33,7 @@ def callback(msg):
description = msg.data


def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True):
def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True):
global description
use_small = use_smallest_joint_limits
use_mimic = True
Expand All @@ -39,62 +53,59 @@ def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=Tr
dependent_joints = {}

if description != "":
robot = xml.dom.minidom.parseString(description)\
.getElementsByTagName('robot')[0]
robot = xml.dom.minidom.parseString(description).getElementsByTagName("robot")[0]

# Find all non-fixed joints
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
jtype = child.getAttribute('type')
if jtype == 'fixed':
if child.localName == "joint":
jtype = child.getAttribute("type")
if jtype == "fixed":
continue
name = child.getAttribute('name')
name = child.getAttribute("name")
try:
limit = child.getElementsByTagName('limit')[0]
limit = child.getElementsByTagName("limit")[0]
except:
continue
if jtype == 'continuous':
if jtype == "continuous":
minval = -pi
maxval = pi
else:
try:
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
minval = float(limit.getAttribute("lower"))
maxval = float(limit.getAttribute("upper"))
except:
continue
try:
maxvel = float(limit.getAttribute('velocity'))
maxvel = float(limit.getAttribute("velocity"))
except:
continue
safety_tags = child.getElementsByTagName('safety_controller')
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 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'))
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
joint = {"min_position": minval, "max_position": maxval}
joint["has_position_limits"] = jtype != "continuous"
joint["max_velocity"] = maxvel
free_joints[name] = joint
return free_joints
Loading