Merge branch 'master' into devel

- Rebase of develop branch from changes in master

.github/workflows/publish.yaml

@@ -16,12 +16,13 @@ jobs:
           fetch-depth: 0
       - name: Install dependencies
         run: |
+          sudo apt update
           sudo apt install python3-sphinx
           python3 -m pip install sphinx-markdown-tables
           python3 -m pip install sphinx-rtd-theme
           python3 -m pip install recommonmark
           python3 -m pip install sphinxcontrib-openapi
-          python3 -m pip install mistune==0.8.4
+          python3 -m pip install mistune
       - name: Build
         run: |
           echo "TODO"

.gitignore

@@ -141,4 +141,7 @@ cython_debug/
 .vscode
 
 #Docs
-docs/build
+docs/build
+
+#tests
+tests/*

README.md

@@ -96,29 +96,47 @@ echo "Installation complete!"
 ###  macOS and Windows (10/11)
 To enable easy use of ROS on these operating systems, it is recommended to use [RoboStack](https://robostack.github.io/); note that ROS 1 (noetic) is recommended at this stage. Please ensure you have [mamba](https://mamba.readthedocs.io/en/latest/installation.html) installed before proceeding. Please follow all required steps for the [RoboStack](https://robostack.github.io/) install (as per their instructions) to enable the smoothest setup on your particular OS.
 ```bash
-# Create and activate a new robostack environment
-mamba create -n robostackenv ros-noetic-desktop python=3.9 -c robostack-staging -c conda-forge --no-channel-priority --override-channels
-mamba activate robostackenv
+# --- Mamba Environment Setup --- #
+# Create and activate a new robostack (ros-env) environment
+mamba create -n ros-env ros-noetic-desktop python=3.9 -c robostack-staging -c conda-forge --no-channel-priority --override-channels
+mamba activate ros-env
 
 # Install some compiler packages
 mamba install compilers cmake pkg-config make ninja
 
+# FOR WINDOWS: Install the Visual Studio command prompt - if you use Visual Studio 2022
+mamba install vs2022_win-64
+
+# --- ARMer Setup --- #
 # Make the armer workspace and clone in armer and armer_msgs packages
+# FOR LINUX/MACOS
 mkdir -p ~/armer_ws/src && cd ~/armer_ws/src 
+# FOR WINDOWS: Assumes you are in the home folder
+mkdir armer_ws\src && cd armer_ws\src
+# Clone in armer and armer_msgs
 git clone https://github.com/qcr/armer.git && git clone https://github.com/qcr/armer_msgs 
-
-# Install all required packages (into robostackenv)
-pip install -r ~/armer_ws/src/armer/requirements.txt
-cd .. && rosdep install --from-paths src --ignore-src -r -y 
+# Install all required packages (into ros-env) - from current directory
+# FOR LINUX/MACOS
+pip install -r armer/requirements.txt
+# FOR WINDOWS
+pip install -r armer\requirements.txt
+# Enter armer_ws folder and run rosdep commands
+cd .. && rosdep init && rosdep update && rosdep install --from-paths src --ignore-src -r -y 
 
 # Make and source the workspace (including environment)
 catkin_make 
-echo "mamba activate robostackenv" >> ~/.bashrc
-echo "source ~/armer_ws/devel/setup.bash" >> ~/.bashrc 
+
+# --- Default Activation of Environment --- #
+# FOR LINUX 
+echo "mamba activate ros-env" >> ~/.bashrc
+
+# FOR MACOS
+echo "mamba activate ros-env" >> ~/.bash_profile
+
+# --- Workspace Source --- #
 source ~/armer_ws/devel/setup.bash
 ```
 
-
 Supported Arms
 ---------------
 Armer relies on the manipulator's ROS driver to communicate with the low level hardware so the the ROS drivers must be started along side Armer. ***NOTE: the below packages are required for control of a real robot - see below for simulation usage instructions***
@@ -138,16 +156,18 @@ For more information on setting up manipulators not listed here see the Armer do
 Usage
 -------
 
-The Armer interface can be launched with the following command for simulation:
+The Armer interface can be launched with the following command for simulation (note, please replace USER with your own username):
 
 > ``` {.sourceCode .bash}
-> roslaunch armer armer.launch config:={PATH_TO_CONFIG_YAML_FILE}
+> # Example is using the panda_sim.yaml. Note, please update the below path if the install directory is different
+> roslaunch armer armer.launch config:=/home/$USER/armer_ws/src/armer/cfg/panda_sim.yaml
 > ```
 
 Alternatively, the Armer interface can be launched for a real robot using the following command (Note that this can also support simulation if you wish via the ***sim*** parameter):
 
 > ``` {.sourceCode .bash}
-> roslaunch armer_{ROBOT_MODEL} robot_bringup.launch config:={PATH_TO_CONFIG_YAML_FILE} sim:={true/false}
+> # Note this example launches the panda model in simulation mode (assumes you have this package cloned, see above)
+> roslaunch armer_panda robot_bringup.launch sim:=true
 > ```
 
 After launching, an arm can be controlled in several ways. Some quick tutorials can be referenced below:

armer/armer.py

@@ -15,6 +15,8 @@
 import yaml
 
 import roboticstoolbox as rtb
+import spatialgeometry as sg
+import spatialmath as sm
 from roboticstoolbox.backends.swift import Swift
 
 from spatialmath.base.argcheck import getvector
@@ -78,6 +80,15 @@ def __init__(
         for robot in self.robots:
             self.backend.add(robot)
 
+            # # TESTING
+            # # Add dummy object for testing
+            # s0 = sg.Sphere(radius=0.05, pose=sm.SE3(0.5, 0, 0.5))
+            # s1 = sg.Sphere(radius=0.05, pose=sm.SE3(0.5, 0, 0.1))
+            # robot.add_collision_obj(s0)
+            # robot.add_collision_obj(s1)
+            # self.backend.add(s0)
+            # self.backend.add(s1)
+
         for readonly, args in self.readonly_backends:
             readonly.launch(**args)
 
@@ -204,6 +215,7 @@ def load(path: str) -> Armer:
 
         logging = config['logging'] if 'logging' in config else {}
         publish_transforms = config['publish_transforms'] if 'publish_transforms' in config else False
+
         return Armer(
             robots=robots,
             backend=backend,
@@ -218,6 +230,7 @@ def run(self) -> None:
         Runs the driver. This is a blocking call.
         """
         self.last_tick = rospy.get_time()
+        rospy.loginfo(f"ARMer Node Running...")
 
         while not rospy.is_shutdown():
             with Timer('ROS', self.log_frequency):
@@ -240,7 +253,7 @@ def run(self) -> None:
                 self.last_tick = current_time
 
 
-if __name__ == '__main__':
-    rospy.init_node('manipulator')
-    manipulator = Armer(publish_transforms=False)
-    manipulator.run()
+# if __name__ == '__main__':
+#     rospy.init_node('manipulator')
+#     manipulator = Armer(publish_transforms=False)
+#     manipulator.run()

armer/models/URDFRobot.py

@@ -1,16 +1,22 @@
 #!/usr/bin/env python
+"""
+URDFRobot module defines the URDFRobot type
+
+.. codeauthor:: Gavin Suddreys
+.. codeauthor:: Dasun Gunasinghe
+"""
 
 import numpy as np
 import re
 import rospy
 import rospkg
 from io import BytesIO
-from roboticstoolbox.robot import ERobot, Link, ET, ETS
+from roboticstoolbox.robot import Robot, Link, ET, ETS
 from roboticstoolbox.tools import URDF
 import xml.etree.ElementTree as ETT
 import spatialmath
 
-class URDFRobot(ERobot):
+class URDFRobot(Robot):
   def __init__(self,
                qz=None,
                qr=None,
@@ -37,6 +43,10 @@ def __init__(self,
 
       gripper_link[0].tool = spatialmath.SE3(ets.compile()[0].A())
 
+    # DEBUGGING
+    # print(f"links:")
+    # for link in links:
+    #   print(link)
 
     super().__init__(
         links,
@@ -53,10 +63,10 @@ def __init__(self,
     self.addconfiguration("qz", self.qz)
 
   def URDF_read_description(self):
-    rospy.loginfo('Waiting for robot description')
+    rospy.loginfo('[INIT] Waiting for robot description')
     while not rospy.has_param('/robot_description'):
       rospy.sleep(0.5)
-    rospy.loginfo('Found robot description')
+    rospy.loginfo('[INIT] Found robot description')
 
     urdf_string = self.URDF_resolve(rospy.get_param('/robot_description'))