ROS progress

jhavl · jhavl · commit 55d2f41e892c · 2021-01-18T16:24:08.000+10:00
diff --git a/examples/ros.py b/examples/ros.py
@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import os
+
+# Launch the simulator Swift
+env = rtb.backends.ROS()
+env.launch(ros_master_uri='http://localhost:11311')
+
+os.system('rostopic list')
+
+# # Create a Panda robot object
+# panda = rtb.models.Panda()
+
+# # Set joint angles to ready configuration
+# panda.q = panda.qr
+
+# # Add the Panda to the simulator
+# env.add(panda)
+
+
diff --git a/roboticstoolbox/backends/ROS/ROS.py b/roboticstoolbox/backends/ROS/ROS.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+from roboticstoolbox.backends.Connector import Connector
+import roboticstoolbox as rtb
+import numpy as np
+import spatialmath as sm
+
+import time
+import subprocess
+import os
+import rospy
+from std_msgs.msg import Float32MultiArray
+
+
+class ROS(Connector):  # pragma nocover
+    """
+
+    """
+    def __init__(self):
+        super(ROS, self).__init__()
+
+        self.robots = []
+
+    #
+    #  Basic methods to do with the state of the external program
+    #
+
+    def launch(self, roscore=False, ros_master_uri=None, ros_ip=None):
+        """
+        """
+
+        super().launch()
+
+        # Launch roscore in seperate process
+        if roscore:
+            print('Launching ROS core\n')
+            self.roscore = subprocess.Popen('roscore')
+
+            # Give it some time to launch
+            time.sleep(1)
+
+        if ros_master_uri:
+            os.environ["ROS_MASTER_URI"] = ros_master_uri
+
+        if ros_ip:
+            os.environ["ROS_IP"] = ros_ip
+
+        v = VelPub(rtb.models.Panda())
+
+    def step(self, dt=0.05):
+        """
+        """
+
+        super().step
+
+    def reset(self):
+        """
+        """
+
+        super().reset
+
+    def restart(self):
+        """
+        """
+
+        super().restart
+
+    def close(self):
+        """
+        """
+
+        super().close()
+
+    def hold(self):
+        """
+        """
+
+        super().hold()
+
+    #
+    #  Methods to interface with the robots created in other environemnts
+    #
+
+    def add(
+            self, ob):
+        """
+
+        """
+
+        super().add()
+
+    def remove(self):
+        """
+        Remove a robot to the graphical scene
+
+        ``env.remove(robot)`` removes the ``robot`` from the graphical
+            environment.
+        """
+
+        # TODO - shouldn't this have an id argument? which robot does it remove
+        # TODO - it can remove any entity?
+
+        super().remove()
+
+
+class VelPub:
+
+    def __init__(self, robot):
+        rospy.init_node('RTB_VelocityPublisher')
+        self.robot = robot
+        self.v = np.zeros(robot.n)
+
+        self.velocity_publisher = rospy.Publisher(
+            '/joint_velocity',
+            Float32MultiArray, queue_size=1)
+
+        self.relay()
+
+    def relay(self):
+
+        while True:
+            data = Float32MultiArray(data=self.robot.q)
+            self.velocity_publisher.publish(data)
diff --git a/roboticstoolbox/backends/ROS/__init__.py b/roboticstoolbox/backends/ROS/__init__.py
@@ -0,0 +1,5 @@
+from roboticstoolbox.backends.ROS.ROS import ROS
+
+__all__ = [
+    'ROS'
+]
diff --git a/roboticstoolbox/backends/__init__.py b/roboticstoolbox/backends/__init__.py
@@ -1,5 +1,6 @@
 from roboticstoolbox.backends.PyPlot import *   # noqa
 from roboticstoolbox.backends.Swift import *   # noqa
+from roboticstoolbox.backends.ROS import *   # noqa
 
 try:
     from roboticstoolbox.backends.VPython import *   # noqa

-Original file line number
+Diff line change
@@ @@ -0,0 +1,5 @@ @@
 +from roboticstoolbox.backends.ROS.ROS import ROS
++
 +__all__ = [
 +    'ROS'
 +]