auto-format python code with black

This commit is contained in:
Robert Haschke 2021-03-16 18:39:18 +01:00
parent 10adb63f4a
commit fd25a0626a
4 changed files with 22 additions and 21 deletions

View File

@ -27,7 +27,7 @@ class Test(unittest.TestCase):
task.add(stages.CurrentState("current")) task.add(stages.CurrentState("current"))
move = stages.MoveRelative("move", core.JointInterpolationPlanner()) move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP move.group = self.PLANNING_GROUP
move.setDirection({"joint_1" : 0.2, "joint_2" : 0.4}) move.setDirection({"joint_1": 0.2, "joint_2": 0.4})
task.add(move) task.add(move)
task.enableIntrospection() task.enableIntrospection()
@ -42,14 +42,12 @@ class Test(unittest.TestCase):
def test_Merger(self): def test_Merger(self):
cartesian = core.CartesianPath() cartesian = core.CartesianPath()
def createDisplacement(group, displacement): def createDisplacement(group, displacement):
move = stages.MoveRelative("displace", cartesian) move = stages.MoveRelative("displace", cartesian)
move.group = group move.group = group
move.ik_frame = PoseStamped(header = Header(frame_id = "tool0")) move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
dir = Vector3Stamped( dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement))
header = Header(frame_id = "base_link"),
vector = Vector3(*displacement)
)
move.setDirection(dir) move.setDirection(dir)
move.restrictDirection(stages.MoveRelative.Direction.FORWARD) move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
return move return move
@ -58,7 +56,7 @@ class Test(unittest.TestCase):
task.add(stages.CurrentState("current")) task.add(stages.CurrentState("current"))
merger = core.Merger("merger") merger = core.Merger("merger")
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0])) merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
merger.insert(createDisplacement(self.PLANNING_GROUP, [ 0.2, 0, 0])) merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0]))
task.add(merger) task.add(merger)
task.enableIntrospection() task.enableIntrospection()
@ -67,6 +65,6 @@ class Test(unittest.TestCase):
task.publish(task.solutions[0]) task.publish(task.solutions[0])
if __name__ == '__main__': if __name__ == "__main__":
roscpp_init("test_mtc") roscpp_init("test_mtc")
rostest.rosrun("", "", Test) rostest.rosrun("", "", Test)

View File

@ -143,9 +143,11 @@ class TestStages(unittest.TestCase):
except TypeError as e: except TypeError as e:
pass pass
except: except:
self.fail("Assigning {} did raise wrong exception: {}".format(value, sys.exc_info()[0])) msg = "Assigning {} did raise wrong exception: {}"
self.fail(msg.format(value, sys.exc_info()[0]))
else: else:
self.fail("Assigning {} did not raise an exception, result: {}".format(value, getattr(stage, name))) msg = "Assigning {} did not raise an exception, result: {}"
self.fail(msg.format(value, getattr(stage, name)))
def test_CurrentState(self): def test_CurrentState(self):
stage = stages.CurrentState("current") stage = stages.CurrentState("current")
@ -181,7 +183,7 @@ class TestStages(unittest.TestCase):
stage.setGoal(PointStamped()) stage.setGoal(PointStamped())
stage.setGoal(RobotState()) stage.setGoal(RobotState())
stage.setGoal("named pose") stage.setGoal("named pose")
stage.setGoal(dict(joint1 = 1.0, joint2 = 2.0)) stage.setGoal(dict(joint1=1.0, joint2=2.0))
self._check(stage, "path_constraints", Constraints()) self._check(stage, "path_constraints", Constraints())
def test_MoveRelative(self): def test_MoveRelative(self):
@ -194,7 +196,7 @@ class TestStages(unittest.TestCase):
self._check(stage, "path_constraints", Constraints()) self._check(stage, "path_constraints", Constraints())
stage.setDirection(TwistStamped()) stage.setDirection(TwistStamped())
stage.setDirection(Vector3Stamped()) stage.setDirection(Vector3Stamped())
stage.setDirection({'joint': 0.1}) stage.setDirection({"joint": 0.1})
def test_Connect(self): def test_Connect(self):
planner = core.PipelinePlanner() planner = core.PipelinePlanner()
@ -290,5 +292,5 @@ class TestTask(unittest.TestCase):
task.add(stages.FixedState()) task.add(stages.FixedState())
if __name__ == '__main__': if __name__ == "__main__":
unittest.main() unittest.main()

View File

@ -5,8 +5,8 @@ from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup( d = generate_distutils_setup(
# list of packages to setup # list of packages to setup
packages = ['moveit', 'moveit.python_tools', 'moveit.task_constructor'], packages=["moveit", "moveit.python_tools", "moveit.task_constructor"],
# specify location of root ('') package dir # specify location of root ("") package dir
package_dir = {'' : 'python/src'} package_dir={"": "python/src"},
) )
setup(**d) setup(**d)

View File

@ -8,6 +8,7 @@ from math import pi
import time import time
from moveit.python_tools import roscpp_init from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial") roscpp_init("mtc_tutorial")
group = "panda_arm" group = "panda_arm"
@ -25,26 +26,26 @@ task.add(stages.CurrentState("current state"))
# move along x # move along x
move = stages.MoveRelative("x +0.2", cartesian) move = stages.MoveRelative("x +0.2", cartesian)
move.group = group move.group = group
header = Header(frame_id = "world") header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2,0,0))) move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
task.add(move) task.add(move)
# move along y # move along y
move = stages.MoveRelative("y -0.3", cartesian) move = stages.MoveRelative("y -0.3", cartesian)
move.group = group move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0,-0.3,0))) move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
task.add(move) task.add(move)
# rotate about z # rotate about z
move = stages.MoveRelative("rz +45°", cartesian) move = stages.MoveRelative("rz +45°", cartesian)
move.group = group move.group = group
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0,0,pi/4.)))) move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
task.add(move) task.add(move)
# Cartesian motion, defined as joint-space offset # Cartesian motion, defined as joint-space offset
move = stages.MoveRelative("joint offset", cartesian) move = stages.MoveRelative("joint offset", cartesian)
move.group = group move.group = group
move.setDirection(dict(panda_joint1=pi/6, panda_joint3=-pi/6)) move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6))
task.add(move) task.add(move)
# moveTo named posture, using joint-space interplation # moveTo named posture, using joint-space interplation