Added config package

Started controller things

src/ar3_config/config/controller_manager.yaml

@@ -0,0 +1,23 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_controller/JointStateBroadcaster
+
+joint_trajectory_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6

src/ar3_config/launch/dev.launch.py

@@ -0,0 +1,63 @@
+from os.path import join
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+
+def generate_launch_description():
+    launch_root = get_package_share_directory('ar3_config')
+    description_root = get_package_share_directory('ar3_description')
+    config_root = join(launch_root, 'config')
+
+    urdf_path = join(description_root, 'ar3.urdf')
+    with open(urdf_path, 'r') as f:
+        robot_description = {'robot_description': f.read()}
+
+    return LaunchDescription([
+        # RViz
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            output='log',
+            arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
+            parameters=[robot_description]),
+
+        # Attatch robot to map
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']),
+
+        # Controller
+        Node(
+            package='controller_manager',
+            executable='ros2_control_node',
+            output='screen',
+            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]),
+
+        # Start controllers
+        ExecuteProcess(
+            cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
+            shell=True,
+            output='screen'),
+        ExecuteProcess(
+            cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
+            shell=True,
+            output='screen'),
+
+        # Robot state publisher
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            output='screen',
+            parameters=[robot_description]),
+
+        # Fake robot state publisher
+        Node(
+            package='ar3_description',
+            executable='state_publisher_net',
+            name='state_publisher_net',
+            emulate_tty=True,
+            output='screen'),
+    ])

src/ar3_config/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ar3_config</name>
+  <version>1.0.0</version>
+  <description>Contains configuration and launch files for the ar3</description>
+  <maintainer email="tmuller2017@my.fit.edu">Thomas Muller</maintainer>
+  <maintainer email="farrell2017@my.fit.edu">John Farrell</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/ar3_config/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/ar3_config
+[install]
+install-scripts=$base/lib/ar3_config

src/ar3_config/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+from glob import glob
+
+package_name = 'ar3_config'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/config', glob('config/*.yaml')),
+        ('share/' + package_name, glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer=['Thomas Muller', 'John Farrell'],
+    maintainer_email=['tmuller2017@my.fit.edu', 'farrell2017@my.fit.edu'],
+    description='Contains configuration and launch files for the ar3',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/ar3_config/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/ar3_config/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/ar3_config/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/ar3_description/urdf/ar3.rviz

@@ -45,43 +45,19 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz_default_plugins/TF
-      Enabled: true
+      Enabled: false
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        base_link:
-          Value: true
-        link_1:
-          Value: true
-        link_2:
-          Value: true
-        link_3:
-          Value: true
-        link_4:
-          Value: true
-        link_5:
-          Value: true
-        link_6:
-          Value: true
-        odom:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: false
       Tree:
-        odom:
-          base_link:
-            link_1:
-              link_2:
-                link_3:
-                  link_4:
-                    link_5:
-                      link_6:
-                        {}
+        {}
       Update Interval: 0
-      Value: true
+      Value: false
     - Alpha: 1
       Class: rviz_default_plugins/RobotModel
       Collision Enabled: false
@@ -183,25 +159,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.0058364868164062
+      Distance: 2.843780755996704
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: -0.0068215844221413136
+        Y: 0.06749681383371353
+        Z: 0.3262268602848053
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0453979969024658
+      Pitch: 0.4247964024543762
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.080399513244629
+      Yaw: 3.475400686264038
     Saved: ~
 Window Geometry:
   Displays:
@@ -217,5 +193,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1200
-  X: 1278
-  Y: 80
+  X: 1321
+  Y: 50

src/ar3_description/urdf/ar3.urdf

@@ -358,4 +358,41 @@
     <axis
       xyz="0 0 1" />
   </joint>
+
+
+ <ros2_control name="joint_trajectory_controller" type="system">
+  <hardware>
+   <plugin>fake_components/GenericSystem</plugin>
+  </hardware>
+  <joint name="joint_1">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+  <joint name="joint_2">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+  <joint name="joint_3">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+  <joint name="joint_4">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+  <joint name="joint_5">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+  <joint name="joint_6">
+   <param name="initial_position">0.0</param>
+   <command_interface name="position" />
+   <state_interface name="position" />
+  </joint>
+ </ros2_control>
 </robot>