diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..36349bc --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,14 @@ +name: Ros2 Build +on: + - push + - pull_request + +jobs: + test: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: build docker container + run: docker buildx build -t local . + - name: rosdep install and colcon build + run: docker run local /bin/bash -c "source /opt/ros/humble/setup.bash && rosdep install --from-paths src -y --ignore-src --rosdistro humble && colcon build --symlink-install" \ No newline at end of file diff --git a/.gitignore b/.gitignore old mode 100644 new mode 100755 index 17744ae..d8c69bf --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,5 @@ CATKIN_IGNORE install/ log/ + +bloodstone.urdf \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..76e9f9a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "~/miniforge3/envs/ros_env/include/**", + "/robotics/bcm2835-1.73/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c17", + "cppStandard": "gnu++17", + "intelliSenseMode": "linux-gcc-arm64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..b6cafdb --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,92 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" + }, + "ros.distro": "humble", + "python.autoComplete.extraPaths": [ + "/workspaces/DevEnv/repos/Robot2023/install/grr_test/lib/python3.10/site-packages", + "/workspaces/DevEnv/repos/Robot2023/install/grr_python_controllers/lib/python3.10/site-packages", + "/workspaces/DevEnv/repos/Robot2023/install/grr_bringup/lib/python3.10/site-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/workspaces/DevEnv/repos/Robot2023/install/grr_test/lib/python3.10/site-packages", + "/workspaces/DevEnv/repos/Robot2023/install/grr_python_controllers/lib/python3.10/site-packages", + "/workspaces/DevEnv/repos/Robot2023/install/grr_bringup/lib/python3.10/site-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..9694f47 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,23 @@ +# This is an auto generated Dockerfile for ros:ros-base +# generated from docker_images_ros2/create_ros_image.Dockerfile.em +FROM ros:humble-ros-base-jammy + +# install bootstrap tools +RUN apt-get update && apt-get install --no-install-recommends -y \ + build-essential \ + git \ + python3-colcon-common-extensions \ + python3-colcon-mixin \ + python3-rosdep \ + python3-vcstool \ + && rm -rf /var/lib/apt/lists/* + +# bootstrap rosdep +RUN rosdep update --rosdistro $ROS_DISTRO +# install ros2 packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base=0.10.0-1* + +COPY src /workspace/src + +WORKDIR /workspace diff --git a/LICENSE b/LICENSE old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/src/grr_bringup/config/controllers.yaml b/src/grr_bringup/config/controllers.yaml new file mode 100755 index 0000000..51dffd6 --- /dev/null +++ b/src/grr_bringup/config/controllers.yaml @@ -0,0 +1,63 @@ +controller_manager: + ros__parameters: + update_rate: 1 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + grr_cmake_controller: + type: grr_cmake_controller/MecanumController + +joint_state_broadcaster: + ros__parameters: + extra_joints: [ + "rear_right_roller_0_joint", + "rear_right_roller_1_joint", + "rear_right_roller_2_joint", + "rear_right_roller_3_joint", + "rear_right_roller_4_joint", + "rear_right_roller_5_joint", + "rear_right_roller_6_joint", + "rear_right_roller_7_joint", + "rear_right_roller_8_joint", + "rear_left_roller_0_joint", + "rear_left_roller_1_joint", + "rear_left_roller_2_joint", + "rear_left_roller_3_joint", + "rear_left_roller_4_joint", + "rear_left_roller_5_joint", + "rear_left_roller_6_joint", + "rear_left_roller_7_joint", + "rear_left_roller_8_joint", + "front_left_roller_0_joint", + "front_left_roller_1_joint", + "front_left_roller_2_joint", + "front_left_roller_3_joint", + "front_left_roller_4_joint", + "front_left_roller_5_joint", + "front_left_roller_6_joint", + "front_left_roller_7_joint", + "front_left_roller_8_joint", + "front_right_roller_0_joint", + "front_right_roller_1_joint", + "front_right_roller_2_joint", + "front_right_roller_3_joint", + "front_right_roller_4_joint", + "front_right_roller_5_joint", + "front_right_roller_6_joint", + "front_right_roller_7_joint", + "front_right_roller_8_joint"] + +grr_cmake_controller: + ros__parameters: + front_left_joint: "front_left_mecanum_joint" + front_right_joint: "front_right_mecanum_joint" + rear_left_joint: "rear_left_mecanum_joint" + rear_right_joint: "rear_right_mecanum_joint" + + chassis_center_to_axle: 0.104775 #4.125 in to m + axle_center_to_wheel: 0.10795 #4.25 in + wheel_radius: 0.0375 #1.5 in + + cmd_vel_timeout: 0.5 + use_stamped_vel: false diff --git a/src/grr_bringup/config/isaac.rviz b/src/grr_bringup/config/isaac.rviz new file mode 100755 index 0000000..1fa4d83 --- /dev/null +++ b/src/grr_bringup/config/isaac.rviz @@ -0,0 +1,690 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.6529411673545837 + Tree Height: 821 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 1 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + zed_camera_center: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + zed_left_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + zed_left_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + zed_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + zed_right_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + zed_right_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + chassis_link: + Value: false + front_left_mecanum_link: + Value: true + front_left_roller_0: + Value: false + front_left_roller_1: + Value: false + front_left_roller_2: + Value: false + front_left_roller_3: + Value: false + front_left_roller_4: + Value: false + front_left_roller_5: + Value: false + front_left_roller_6: + Value: false + front_left_roller_7: + Value: false + front_left_roller_8: + Value: false + front_right_mecanum_link: + Value: true + front_right_roller_0: + Value: false + front_right_roller_1: + Value: false + front_right_roller_2: + Value: false + front_right_roller_3: + Value: false + front_right_roller_4: + Value: false + front_right_roller_5: + Value: false + front_right_roller_6: + Value: false + front_right_roller_7: + Value: false + front_right_roller_8: + Value: false + lidar_link: + Value: false + rear_left_mecanum_link: + Value: true + rear_left_roller_0: + Value: false + rear_left_roller_1: + Value: false + rear_left_roller_2: + Value: false + rear_left_roller_3: + Value: false + rear_left_roller_4: + Value: false + rear_left_roller_5: + Value: false + rear_left_roller_6: + Value: false + rear_left_roller_7: + Value: false + rear_left_roller_8: + Value: false + rear_right_mecanum_link: + Value: true + rear_right_roller_0: + Value: false + rear_right_roller_1: + Value: false + rear_right_roller_2: + Value: false + rear_right_roller_3: + Value: false + rear_right_roller_4: + Value: false + rear_right_roller_5: + Value: false + rear_right_roller_6: + Value: false + rear_right_roller_7: + Value: false + rear_right_roller_8: + Value: false + zed_camera_center: + Value: false + zed_left_camera_frame: + Value: false + zed_left_camera_optical_frame: + Value: false + zed_mount_link: + Value: false + zed_right_camera_frame: + Value: false + zed_right_camera_optical_frame: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + chassis_link: + front_left_mecanum_link: + front_left_roller_0: + {} + front_left_roller_1: + {} + front_left_roller_2: + {} + front_left_roller_3: + {} + front_left_roller_4: + {} + front_left_roller_5: + {} + front_left_roller_6: + {} + front_left_roller_7: + {} + front_left_roller_8: + {} + front_right_mecanum_link: + front_right_roller_0: + {} + front_right_roller_1: + {} + front_right_roller_2: + {} + front_right_roller_3: + {} + front_right_roller_4: + {} + front_right_roller_5: + {} + front_right_roller_6: + {} + front_right_roller_7: + {} + front_right_roller_8: + {} + lidar_link: + {} + rear_left_mecanum_link: + rear_left_roller_0: + {} + rear_left_roller_1: + {} + rear_left_roller_2: + {} + rear_left_roller_3: + {} + rear_left_roller_4: + {} + rear_left_roller_5: + {} + rear_left_roller_6: + {} + rear_left_roller_7: + {} + rear_left_roller_8: + {} + rear_right_mecanum_link: + rear_right_roller_0: + {} + rear_right_roller_1: + {} + rear_right_roller_2: + {} + rear_right_roller_3: + {} + rear_right_roller_4: + {} + rear_right_roller_5: + {} + rear_right_roller_6: + {} + rear_right_roller_7: + {} + rear_right_roller_8: + {} + zed_mount_link: + zed_camera_center: + zed_left_camera_frame: + zed_left_camera_optical_frame: + {} + zed_right_camera_frame: + zed_right_camera_optical_frame: + {} + Update Interval: 0 + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: -999999 + Min Color: 0; 0; 0 + Min Intensity: 999999 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /laser_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rgb + Value: true + Visibility: + Grid: true + LaserScan: true + Odometry: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.2246923446655273 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.02185363881289959 + Y: 0.06152752786874771 + Z: -0.06119866296648979 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2953988313674927 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.123585224151611 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1403 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004ddfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c0000000c900fffffffb0000000c00430061006d0065007200610100000403000001170000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000046d000000ad0000000000000000000000010000010f000004ddfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004dd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000747000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 72 + Y: 0 diff --git a/src/grr_bringup/config/servo_params.yaml b/src/grr_bringup/config/servo_params.yaml new file mode 100644 index 0000000..4accf2d --- /dev/null +++ b/src/grr_bringup/config/servo_params.yaml @@ -0,0 +1,25 @@ +grr/robot: + ros__parameters: + pca_address: 0x40 + pca_frequency: 100 + servo_mapping: + bridge_latch_joint: + port: 15 + maximum: 4850 + minimum: 3500 + mechanism_lift_joint: + port: 14 + maximum: 0 + minimum: 0 + mechanism_package_joint: + port: 13 + maximum: 11316 + minimum: 6880 + mechanism_thruster_joint: + port: 12 + maximum: 11500 + minimum: 3000 + small_package_sweeper_joint: + port: 11 + maximum: 0 + minimum: 0 \ No newline at end of file diff --git a/src/grr_bringup/config/view.rviz b/src/grr_bringup/config/view.rviz new file mode 100755 index 0000000..91070e9 --- /dev/null +++ b/src/grr_bringup/config/view.rviz @@ -0,0 +1,554 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 366 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_mecanum_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_roller_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + small_package_grabber_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + small_package_grabber_roller_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + small_package_grabber_roller_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + small_package_grabber_roller_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + small_package_grabber_roller_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + chassis_link: + Value: false + front_left_mecanum_link: + Value: false + front_left_roller_0: + Value: false + front_left_roller_1: + Value: false + front_left_roller_2: + Value: false + front_left_roller_3: + Value: false + front_left_roller_4: + Value: false + front_left_roller_5: + Value: false + front_left_roller_6: + Value: false + front_left_roller_7: + Value: false + front_right_mecanum_link: + Value: false + front_right_roller_0: + Value: false + front_right_roller_1: + Value: false + front_right_roller_2: + Value: false + front_right_roller_3: + Value: false + front_right_roller_4: + Value: false + front_right_roller_5: + Value: false + front_right_roller_6: + Value: false + front_right_roller_7: + Value: false + rear_left_mecanum_link: + Value: false + rear_left_roller_0: + Value: false + rear_left_roller_1: + Value: false + rear_left_roller_2: + Value: false + rear_left_roller_3: + Value: false + rear_left_roller_4: + Value: false + rear_left_roller_5: + Value: false + rear_left_roller_6: + Value: false + rear_left_roller_7: + Value: false + rear_right_mecanum_link: + Value: false + rear_right_roller_0: + Value: false + rear_right_roller_1: + Value: false + rear_right_roller_2: + Value: false + rear_right_roller_3: + Value: false + rear_right_roller_4: + Value: false + rear_right_roller_5: + Value: false + rear_right_roller_6: + Value: false + rear_right_roller_7: + Value: false + small_package_grabber_link: + Value: false + small_package_grabber_roller_1_link: + Value: false + small_package_grabber_roller_2_link: + Value: false + small_package_grabber_roller_3_link: + Value: false + small_package_grabber_roller_4_link: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + chassis_link: + front_left_mecanum_link: + front_left_roller_0: + {} + front_left_roller_1: + {} + front_left_roller_2: + {} + front_left_roller_3: + {} + front_left_roller_4: + {} + front_left_roller_5: + {} + front_left_roller_6: + {} + front_left_roller_7: + {} + front_right_mecanum_link: + front_right_roller_0: + {} + front_right_roller_1: + {} + front_right_roller_2: + {} + front_right_roller_3: + {} + front_right_roller_4: + {} + front_right_roller_5: + {} + front_right_roller_6: + {} + front_right_roller_7: + {} + rear_left_mecanum_link: + rear_left_roller_0: + {} + rear_left_roller_1: + {} + rear_left_roller_2: + {} + rear_left_roller_3: + {} + rear_left_roller_4: + {} + rear_left_roller_5: + {} + rear_left_roller_6: + {} + rear_left_roller_7: + {} + rear_right_mecanum_link: + rear_right_roller_0: + {} + rear_right_roller_1: + {} + rear_right_roller_2: + {} + rear_right_roller_3: + {} + rear_right_roller_4: + {} + rear_right_roller_5: + {} + rear_right_roller_6: + {} + rear_right_roller_7: + {} + small_package_grabber_link: + small_package_grabber_roller_1_link: + {} + small_package_grabber_roller_2_link: + {} + small_package_grabber_roller_3_link: + {} + small_package_grabber_roller_4_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: chassis_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.48347970843315125 + 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 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4953977167606354 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.3904128074645996 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 657 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000001f7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001f7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000001f7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005540000003efc0100000002fb0000000800540069006d00650100000000000005540000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002e3000001f700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1364 + X: 462 + Y: 313 diff --git a/src/grr_bringup/config/xbox-holonomic.config.yaml b/src/grr_bringup/config/xbox-holonomic.config.yaml new file mode 100755 index 0000000..3341a51 --- /dev/null +++ b/src/grr_bringup/config/xbox-holonomic.config.yaml @@ -0,0 +1,15 @@ +teleop_twist_joy_node: + ros__parameters: + require_enable_button: false + axis_linear: # Left thumb stick vertical + x: 1 + y: 0 + scale_linear: + x: 0.7 + y: 0.7 + + axis_angular: # Left thumb stick horizontal + yaw: 3 + scale_angular: + yaw: 1.5 + diff --git a/src/grr_bringup/grr_bringup/__init__.py b/src/grr_bringup/grr_bringup/__init__.py old mode 100644 new mode 100755 diff --git a/src/grr_bringup/launch/competition_robot.launch.py b/src/grr_bringup/launch/competition_robot.launch.py new file mode 100644 index 0000000..57aee6a --- /dev/null +++ b/src/grr_bringup/launch/competition_robot.launch.py @@ -0,0 +1,37 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + servo_params = os.path.join( + get_package_share_directory('grr_bringup'), + 'config', + 'servo_params.yaml' + ) + + print(f"HELLO {servo_params}") + ld = LaunchDescription() + + ld.add_action( + Node( + package='grr_hardware_python', + executable='robot', + name='robot', + output='screen', + emulate_tty=True, + namespace='grr', + parameters=[servo_params] + ) + ) + + + return ld + + diff --git a/src/grr_bringup/launch/isaac.launch.py b/src/grr_bringup/launch/isaac.launch.py new file mode 100755 index 0000000..c97d0b9 --- /dev/null +++ b/src/grr_bringup/launch/isaac.launch.py @@ -0,0 +1,124 @@ +import os +from ament_index_python.packages import get_package_prefix, get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +import xacro + +def generate_launch_description(): + + # Get Local Files + description_pkg_path = os.path.join(get_package_share_directory('grr_description')) + config_pkg_path = os.path.join(get_package_share_directory('grr_bringup')) + joystick_file = os.path.join(config_pkg_path, 'config', 'xbox-holonomic.config.yaml') + xacro_file = os.path.join(description_pkg_path, 'urdf', 'robots','bloodstone.urdf.xacro') + controllers_file = os.path.join(config_pkg_path, 'config', 'controllers.yaml') + print(controllers_file) + rviz_file = os.path.join(config_pkg_path, 'config', 'config.rviz') + robot_description_config = xacro.process_file(xacro_file) + robot_description_xml = robot_description_config.toxml() + print(robot_description_xml) + source_code_path = os.path.abspath(os.path.join(description_pkg_path, "../../../../src/Robot2023/src/grr_description")) + urdf_save_path = os.path.join(source_code_path, "bloodstone.urdf") + + with open(urdf_save_path, 'w') as f: + f.write(robot_description_xml) + # Create a robot_state_publisher node + description_params = {'robot_description': robot_description_xml, 'use_sim_time': True } + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[description_params] + ) + + + # Starts ROS2 Control + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[{'robot_description': robot_description_xml, 'use_sim_time': True }, controllers_file], + output="screen", + ) + + + # Starts ROS2 Control Joint State Broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + + + # Starts ROS2 Control Mecanum Drive Controller + mecanum_drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["grr_cmake_controller", "-c", "/controller_manager"], + ) + mecanum_drive_controller_delay = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[mecanum_drive_controller_spawner], + ) + ) + + + # Start Rviz2 with basic view + run_rviz2_node = Node( + package='rviz2', + executable='rviz2', + parameters=[{ 'use_sim_time': True }], + name='isaac_rviz2', + output='screen', + arguments=[["-d"], [rviz_file]], + ) + + + # run_rviz2 = ExecuteProcess( + # cmd=['rviz2', '-d', rviz_file], + # output='screen' + # ) + rviz2_delay = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[run_rviz2_node], + ) + ) + + + # Start Joystick Node + joy = Node( + package='joy', + executable='joy_node', + name='joy_node', + parameters=[{ + 'dev': '/dev/input/js0', + 'deadzone': 0.3, + 'autorepeat_rate': 20.0, + }]) + + + # Start Teleop Node to translate joystick commands to robot commands + joy_teleop = Node( + package='teleop_twist_joy', + executable='teleop_node', + name='teleop_twist_joy_node', + parameters=[joystick_file], + remappings={('/cmd_vel', '/grr_cmake_controller/cmd_vel_unstamped')} + ) + + + # Launch! + return LaunchDescription([ + control_node, + node_robot_state_publisher, + joint_state_broadcaster_spawner, + mecanum_drive_controller_delay, + # rviz2_delay, + joy, + joy_teleop + ]) \ No newline at end of file diff --git a/src/grr_bringup/launch/rviz.launch.py b/src/grr_bringup/launch/rviz.launch.py new file mode 100755 index 0000000..29d9e21 --- /dev/null +++ b/src/grr_bringup/launch/rviz.launch.py @@ -0,0 +1,49 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch_ros.actions import Node + +import xacro + +def generate_launch_description(): + + # Check if we're told to use sim time + use_sim_time = LaunchConfiguration('use_sim_time') + + # Process the URDF file + pkg_path = os.path.join(get_package_share_directory('grr_description')) + xacro_file = os.path.join(pkg_path,'urdf', 'robots','bloodstone.urdf.xacro') + grr_description_config = xacro.process_file(xacro_file) + + print(grr_description_config.toxml()) + + # Create a robot_state_publisher node + params = {'robot_description': grr_description_config.toxml(), 'use_sim_time': use_sim_time} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + # Start Rviz2 with basic view + # rviz2_config_path = os.path.join(get_package_share_directory('grr_description'), 'config/isaac.rviz') + # run_rviz2 = ExecuteProcess( + # cmd=['rviz2', '-d', rviz2_config_path], + # output='screen' + # ) + + # Launch! + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use sim time if true'), + + node_robot_state_publisher, + # run_rviz2 + ]) diff --git a/src/grr_bringup/package.xml b/src/grr_bringup/package.xml old mode 100644 new mode 100755 diff --git a/src/grr_bringup/resource/grr_bringup b/src/grr_bringup/resource/grr_bringup old mode 100644 new mode 100755 diff --git a/src/grr_bringup/setup.cfg b/src/grr_bringup/setup.cfg old mode 100644 new mode 100755 diff --git a/src/grr_bringup/setup.py b/src/grr_bringup/setup.py old mode 100644 new mode 100755 index d6d02a7..c77049a --- a/src/grr_bringup/setup.py +++ b/src/grr_bringup/setup.py @@ -1,4 +1,7 @@ from setuptools import setup +import os +from glob import glob + package_name = 'grr_bringup' @@ -10,6 +13,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.*'))), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) ], install_requires=['setuptools'], zip_safe=True, diff --git a/src/grr_bringup/test/test_copyright.py b/src/grr_bringup/test/test_copyright.py old mode 100644 new mode 100755 diff --git a/src/grr_bringup/test/test_flake8.py b/src/grr_bringup/test/test_flake8.py old mode 100644 new mode 100755 diff --git a/src/grr_bringup/test/test_pep257.py b/src/grr_bringup/test/test_pep257.py old mode 100644 new mode 100755 diff --git a/src/grr_cmake_controller/CMakeLists.txt b/src/grr_cmake_controller/CMakeLists.txt old mode 100644 new mode 100755 index 67f0c06..ab06307 --- a/src/grr_cmake_controller/CMakeLists.txt +++ b/src/grr_cmake_controller/CMakeLists.txt @@ -1,26 +1,76 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(grr_cmake_controller) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra) endif() -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() +find_package(controller_interface REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) + +add_library(grr_cmake_controller SHARED + src/grr_cmake_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) + +target_include_directories(grr_cmake_controller PRIVATE include) +ament_target_dependencies(grr_cmake_controller + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(grr_cmake_controller PRIVATE "GRR_CMAKE_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface mecanum_plugin.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS grr_cmake_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle + tf2 + tf2_msgs +) +ament_export_include_directories( + include +) +ament_export_libraries( + grr_cmake_controller +) ament_package() diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/grr_cmake_controller.hpp b/src/grr_cmake_controller/include/grr_cmake_controller/grr_cmake_controller.hpp new file mode 100755 index 0000000..3a756e5 --- /dev/null +++ b/src/grr_cmake_controller/include/grr_cmake_controller/grr_cmake_controller.hpp @@ -0,0 +1,132 @@ +// Copyright 2020 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. + +/* + * Author: Bence Magyar, Enrique Fernández, Manuel Meraz + */ + +#ifndef GRR_CMAKE_CONTROLLER__GRR_CMAKE_CONTROLLER_HPP_ +#define GRR_CMAKE_CONTROLLER__GRR_CMAKE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "grr_cmake_controller/visibility_control.h" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include + +namespace grr_cmake_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + +class Wheel { + public: + Wheel(std::reference_wrapper velocity, std::string name); + void set_velocity(double velocity); + + private: + std::reference_wrapper velocity_; + std::string name; +}; + +class MecanumController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::TwistStamped; + +public: + GRR_CMAKE_CONTROLLER_PUBLIC + MecanumController(); + + GRR_CMAKE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GRR_CMAKE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRR_CMAKE_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + GRR_CMAKE_CONTROLLER_PUBLIC + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::shared_ptr get_wheel(const std::string & wheel_name); + std::shared_ptr front_left_handle_; + std::shared_ptr front_right_handle_; + std::shared_ptr rear_left_handle_; + std::shared_ptr rear_right_handle_; + std::string front_left_joint_name_; + std::string front_right_joint_name_; + std::string rear_left_joint_name_; + std::string rear_right_joint_name_; + + struct WheelParams + { + double x_offset = 0.0; // Chassis Center to Axle Center + double y_offset = 0.0; // Axle Center to Wheel Center + double radius = 0.0; // Assumed to be the same for all wheels + } wheel_params_; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + rclcpp::Time previous_update_timestamp_{0}; + + // Topic Subscription + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + bool reset(); + void halt(); +}; +} // namespace grr_cmake_controller +#endif // GRR_CMAKE_CONTROLLER__GRR_CMAKE_CONTROLLER_HPP_ diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/odometry.hpp b/src/grr_cmake_controller/include/grr_cmake_controller/odometry.hpp new file mode 100755 index 0000000..a1bafeb --- /dev/null +++ b/src/grr_cmake_controller/include/grr_cmake_controller/odometry.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 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. + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#ifndef GRR_CMAKE_CONTROLLER__ODOMETRY_HPP_ +#define GRR_CMAKE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "grr_cmake_controller/rolling_mean_accumulator.hpp" +#include "rclcpp/time.hpp" + +namespace grr_cmake_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + void init(const rclcpp::Time & time); + bool update(double left_pos, double right_pos, const rclcpp::Time & time); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); + void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = grr_cmake_controller::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheel_separation_; + double left_wheel_radius_; + double right_wheel_radius_; + + // Previous wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace grr_cmake_controller + +#endif // GRR_CMAKE_CONTROLLER__ODOMETRY_HPP_ diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/rolling_mean_accumulator.hpp b/src/grr_cmake_controller/include/grr_cmake_controller/rolling_mean_accumulator.hpp new file mode 100755 index 0000000..22b698e --- /dev/null +++ b/src/grr_cmake_controller/include/grr_cmake_controller/rolling_mean_accumulator.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 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. + +/* + * Author: Víctor López + */ + +#ifndef GRR_CMAKE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ +#define GRR_CMAKE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ + +#include +#include +#include + +namespace grr_cmake_controller +{ +/** + * \brief Simplification of boost::accumulators::accumulator_set> to avoid dragging boost dependencies + * + * Computes the mean of the last accumulated elements + */ +template +class RollingMeanAccumulator +{ +public: + explicit RollingMeanAccumulator(size_t rolling_window_size) + : buffer_(rolling_window_size, 0.0), next_insert_(0), sum_(0.0), buffer_filled_(false) + { + } + + void accumulate(T val) + { + sum_ -= buffer_[next_insert_]; + sum_ += val; + buffer_[next_insert_] = val; + next_insert_++; + buffer_filled_ |= next_insert_ >= buffer_.size(); + next_insert_ = next_insert_ % buffer_.size(); + } + + T getRollingMean() const + { + size_t valid_data_count = buffer_filled_ * buffer_.size() + !buffer_filled_ * next_insert_; + assert(valid_data_count > 0); + return sum_ / valid_data_count; + } + +private: + std::vector buffer_; + size_t next_insert_; + T sum_; + bool buffer_filled_; +}; +} // namespace grr_cmake_controller +#endif // GRR_CMAKE_CONTROLLER__ROLLING_MEAN_ACCUMULATOR_HPP_ diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/speed_limiter.hpp b/src/grr_cmake_controller/include/grr_cmake_controller/speed_limiter.hpp new file mode 100755 index 0000000..d6f44c1 --- /dev/null +++ b/src/grr_cmake_controller/include/grr_cmake_controller/speed_limiter.hpp @@ -0,0 +1,105 @@ +// Copyright 2020 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. + +/* + * Author: Enrique Fernández + */ + +#ifndef GRR_CMAKE_CONTROLLER__SPEED_LIMITER_HPP_ +#define GRR_CMAKE_CONTROLLER__SPEED_LIMITER_HPP_ + +#include + +namespace grr_cmake_controller +{ +class SpeedLimiter +{ +public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, bool has_acceleration_limits = false, + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits_; + bool has_acceleration_limits_; + bool has_jerk_limits_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace grr_cmake_controller + +#endif // GRR_CMAKE_CONTROLLER__SPEED_LIMITER_HPP_ diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/visibility_control.h b/src/grr_cmake_controller/include/grr_cmake_controller/visibility_control.h new file mode 100755 index 0000000..7c173a4 --- /dev/null +++ b/src/grr_cmake_controller/include/grr_cmake_controller/visibility_control.h @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef GRR_CMAKE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define GRR_CMAKE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRR_CMAKE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRR_CMAKE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRR_CMAKE_CONTROLLER_EXPORT __declspec(dllexport) +#define GRR_CMAKE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRR_CMAKE_CONTROLLER_BUILDING_DLL +#define GRR_CMAKE_CONTROLLER_PUBLIC GRR_CMAKE_CONTROLLER_EXPORT +#else +#define GRR_CMAKE_CONTROLLER_PUBLIC GRR_CMAKE_CONTROLLER_IMPORT +#endif +#define GRR_CMAKE_CONTROLLER_PUBLIC_TYPE GRR_CMAKE_CONTROLLER_PUBLIC +#define GRR_CMAKE_CONTROLLER_LOCAL +#else +#define GRR_CMAKE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRR_CMAKE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRR_CMAKE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRR_CMAKE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRR_CMAKE_CONTROLLER_PUBLIC +#define GRR_CMAKE_CONTROLLER_LOCAL +#endif +#define GRR_CMAKE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GRR_CMAKE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/src/grr_cmake_controller/mecanum_plugin.xml b/src/grr_cmake_controller/mecanum_plugin.xml new file mode 100755 index 0000000..c4b9839 --- /dev/null +++ b/src/grr_cmake_controller/mecanum_plugin.xml @@ -0,0 +1,7 @@ + + + + The differential mecanum controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot. + + + diff --git a/src/grr_cmake_controller/package.xml b/src/grr_cmake_controller/package.xml old mode 100644 new mode 100755 diff --git a/src/grr_cmake_controller/src/grr_cmake_controller.cpp b/src/grr_cmake_controller/src/grr_cmake_controller.cpp new file mode 100755 index 0000000..f065f34 --- /dev/null +++ b/src/grr_cmake_controller/src/grr_cmake_controller.cpp @@ -0,0 +1,356 @@ +// Copyright 2020 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. + +/* + * Author: Bence Magyar, Enrique Fernández, Manuel Meraz + */ + +#include +#include +#include +#include +#include + +#include "grr_cmake_controller/grr_cmake_controller.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +} // namespace + +namespace grr_cmake_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +Wheel::Wheel(std::reference_wrapper velocity, + std::string name) : velocity_(velocity), name(std::move(name)) {} + +void Wheel::set_velocity(double velocity) +{ + velocity_.get().set_value(velocity); +} + +MecanumController::MecanumController() : controller_interface::ControllerInterface() {} + +CallbackReturn MecanumController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare("front_left_joint", front_left_joint_name_); + auto_declare("front_right_joint", front_right_joint_name_); + auto_declare("rear_left_joint", rear_left_joint_name_); + auto_declare("rear_right_joint", rear_right_joint_name_); + + auto_declare("chassis_center_to_axle", wheel_params_.x_offset); + auto_declare("axle_center_to_wheel", wheel_params_.y_offset); + auto_declare("wheel_radius", wheel_params_.radius); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); + auto_declare("use_stamped_vel", use_stamped_vel_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration MecanumController::command_interface_configuration() const +{ + std::vector conf_names; + conf_names.push_back(front_left_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(front_right_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_left_joint_name_ + "/" + HW_IF_VELOCITY); + conf_names.push_back(rear_right_joint_name_ + "/" + HW_IF_VELOCITY); + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration MecanumController::state_interface_configuration() const +{ + return {interface_configuration_type::NONE}; +} + +controller_interface::return_type MecanumController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto logger = get_node()->get_logger(); + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + + const auto current_time = time; + + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); + + if (last_command_msg == nullptr) + { + RCLCPP_WARN(logger, "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = current_time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + Twist command = *last_command_msg; + double & linear_x_cmd = command.twist.linear.x; + double & linear_y_cmd = command.twist.linear.y; + double & angular_cmd = command.twist.angular.z; + + double x_offset = wheel_params_.x_offset; + double y_offset = wheel_params_.y_offset; + double radius = wheel_params_.radius; + + // Compute Wheel Velocities + const double front_left_offset = (-1 * x_offset + -1 * y_offset); + const double front_right_offset = (x_offset + y_offset); + const double rear_left_offset = (-1 * x_offset + -1 * y_offset); + const double rear_right_offset = (x_offset + y_offset); + + const double front_left_velocity = (front_left_offset * angular_cmd + linear_x_cmd - linear_y_cmd) / radius; + const double front_right_velocity = (front_right_offset * angular_cmd + linear_x_cmd + linear_y_cmd) / radius; + const double rear_left_velocity = (rear_left_offset * angular_cmd + linear_x_cmd + linear_y_cmd) / radius; + const double rear_right_velocity = (rear_right_offset * angular_cmd + linear_x_cmd - linear_y_cmd) / radius; + + // Set Wheel Velocities + front_left_handle_->set_velocity(front_left_velocity); + front_right_handle_->set_velocity(front_right_velocity); + rear_left_handle_->set_velocity(rear_left_velocity); + rear_right_handle_->set_velocity(rear_right_velocity); + + // Time update + const auto update_dt = current_time - previous_update_timestamp_; + previous_update_timestamp_ = current_time; + + return controller_interface::return_type::OK; +} + +CallbackReturn MecanumController::on_configure(const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + // Get Parameters + front_left_joint_name_ = get_node()->get_parameter("front_left_joint").as_string(); + front_right_joint_name_ = get_node()->get_parameter("front_right_joint").as_string(); + rear_left_joint_name_ = get_node()->get_parameter("rear_left_joint").as_string(); + rear_right_joint_name_ = get_node()->get_parameter("rear_right_joint").as_string(); + + if (front_left_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_left_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (front_right_joint_name_.empty()) { + RCLCPP_ERROR(logger, "front_right_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (rear_left_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_left_joint_name is not set"); + return CallbackReturn::ERROR; + } + if (rear_right_joint_name_.empty()) { + RCLCPP_ERROR(logger, "rear_right_joint_name is not set"); + return CallbackReturn::ERROR; + } + + wheel_params_.x_offset = get_node()->get_parameter("chassis_center_to_axle").as_double(); + wheel_params_.y_offset = get_node()->get_parameter("axle_center_to_wheel").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + + cmd_vel_timeout_ = std::chrono::milliseconds{ + static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + + // Run reset to make sure everything is initialized correctly + if (!reset()) + { + return CallbackReturn::ERROR; + } + + const Twist empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // initialize command subscriber + if (use_stamped_vel_) + { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); + } + else + { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) + { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + + previous_update_timestamp_ = get_node()->get_clock()->now(); + return CallbackReturn::SUCCESS; +} + +CallbackReturn MecanumController::on_activate(const rclcpp_lifecycle::State &) +{ + front_left_handle_ = get_wheel(front_left_joint_name_); + front_right_handle_ = get_wheel(front_right_joint_name_); + rear_left_handle_ = get_wheel(rear_left_joint_name_); + rear_right_handle_ = get_wheel(rear_right_joint_name_); + + if (!front_left_handle_ || !front_right_handle_ || !rear_left_handle_ || !rear_right_handle_) + { + return CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn MecanumController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn MecanumController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn MecanumController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool MecanumController::reset() +{ + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + return true; +} + +CallbackReturn MecanumController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void MecanumController::halt() +{ + front_left_handle_->set_velocity(0.0); + front_right_handle_->set_velocity(0.0); + rear_left_handle_->set_velocity(0.0); + rear_right_handle_->set_velocity(0.0); + auto logger = get_node()->get_logger(); + RCLCPP_WARN(logger, "-----HALT CALLED : STOPPING ALL MOTORS-----"); +} + +std::shared_ptr MecanumController::get_wheel( const std::string & wheel_name ) +{ + auto logger = get_node()->get_logger(); + if (wheel_name.empty()) + { + RCLCPP_ERROR(logger, "Wheel joint name not given. Make sure all joints are specified."); + return nullptr; + } + + // Get Command Handle for joint + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto &interface) + { + return interface.get_name() == wheel_name + "/velocity" && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return nullptr; + } + auto cmd_interface_name = command_handle->get_name(); + RCLCPP_INFO(logger, "FOUND! wheel cmd interface %s", cmd_interface_name.c_str()); + return std::make_shared(std::ref(*command_handle), wheel_name); +} +} // namespace grr_cmake_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + grr_cmake_controller::MecanumController, controller_interface::ControllerInterface) diff --git a/src/grr_cmake_controller/src/odometry.cpp b/src/grr_cmake_controller/src/odometry.cpp new file mode 100755 index 0000000..4e167b2 --- /dev/null +++ b/src/grr_cmake_controller/src/odometry.cpp @@ -0,0 +1,165 @@ +// Copyright 2020 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. + +/* + * Author: Enrique Fernández + */ + +#include "grr_cmake_controller/odometry.hpp" + +namespace grr_cmake_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_separation_(0.0), + left_wheel_radius_(0.0), + right_wheel_radius_(0.0), + left_wheel_old_pos_(0.0), + right_wheel_old_pos_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +void Odometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; +} + +bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & time) +{ + // We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + // Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * left_wheel_radius_; + const double right_wheel_cur_pos = right_pos * right_wheel_radius_; + + // Estimate velocity of wheels using old and current position: + const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + // Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time); + + return true; +} + +bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Compute linear and angular diff: + const double linear = (left_vel + right_vel) * 0.5; + // Now there is a bug about scout angular velocity + const double angular = (right_vel - left_vel) / wheel_separation_; + + // Integrate odometry: + integrateExact(linear, angular); + + timestamp_ = time; + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(linear / dt); + angular_accumulator_.accumulate(angular / dt); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrateExact(linear * dt, angular * dt); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setWheelParams( + double wheel_separation, double left_wheel_radius, double right_wheel_radius) +{ + wheel_separation_ = wheel_separation; + left_wheel_radius_ = left_wheel_radius; + right_wheel_radius_ = right_wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace grr_cmake_controller diff --git a/src/grr_cmake_controller/src/speed_limiter.cpp b/src/grr_cmake_controller/src/speed_limiter.cpp new file mode 100755 index 0000000..8b5c184 --- /dev/null +++ b/src/grr_cmake_controller/src/speed_limiter.cpp @@ -0,0 +1,140 @@ +// Copyright 2020 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. + +/* + * Author: Enrique Fernández + */ + +#include +#include + +#include "grr_cmake_controller/speed_limiter.hpp" +#include "rcppmath/clamp.hpp" + +namespace grr_cmake_controller +{ +SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, + double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, + double max_jerk) +: has_velocity_limits_(has_velocity_limits), + has_acceleration_limits_(has_acceleration_limits), + has_jerk_limits_(has_jerk_limits), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + // Check if limits are valid, max must be specified, min defaults to -max if unspecified + if (has_velocity_limits_) + { + if (std::isnan(max_velocity_)) + { + throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); + } + if (std::isnan(min_velocity_)) + { + min_velocity_ = -max_velocity_; + } + } + if (has_acceleration_limits_) + { + if (std::isnan(max_acceleration_)) + { + throw std::runtime_error( + "Cannot apply acceleration limits if max_acceleration is not specified"); + } + if (std::isnan(min_acceleration_)) + { + min_acceleration_ = -max_acceleration_; + } + } + if (has_jerk_limits_) + { + if (std::isnan(max_jerk_)) + { + throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); + } + if (std::isnan(min_jerk_)) + { + min_jerk_ = -max_jerk_; + } + } +} + +double SpeedLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + if (has_velocity_limits_) + { + v = rcppmath::clamp(v, min_velocity_, max_velocity_); + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + if (has_acceleration_limits_) + { + const double dv_min = min_acceleration_ * dt; + const double dv_max = max_acceleration_ * dt; + + const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (has_jerk_limits_) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace grr_cmake_controller diff --git a/src/grr_description/CMakeLists.txt b/src/grr_description/CMakeLists.txt old mode 100644 new mode 100755 index 572289a..94583e3 --- a/src/grr_description/CMakeLists.txt +++ b/src/grr_description/CMakeLists.txt @@ -11,15 +11,19 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) +install( + DIRECTORY meshes urdf + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/src/grr_description/bloodstone.urdf b/src/grr_description/bloodstone.urdf new file mode 100644 index 0000000..c2cf965 --- /dev/null +++ b/src/grr_description/bloodstone.urdf @@ -0,0 +1,935 @@ +grr_hardware/IsaacDriveHardware3.03.0-11-11-11-11Gazebo/Blue \ No newline at end of file diff --git a/src/grr_description/examplo.urdf b/src/grr_description/examplo.urdf new file mode 100755 index 0000000..b376ec5 --- /dev/null +++ b/src/grr_description/examplo.urdf @@ -0,0 +1,935 @@ +grr_hardware/IsaacDriveHardware3.03.0-11-11-11-11Gazebo/Blue \ No newline at end of file diff --git a/src/grr_description/meshes/april_tag_mount.stl b/src/grr_description/meshes/april_tag_mount.stl new file mode 100644 index 0000000..a88ccc8 Binary files /dev/null and b/src/grr_description/meshes/april_tag_mount.stl differ diff --git a/src/grr_description/meshes/brain.stl b/src/grr_description/meshes/brain.stl new file mode 100644 index 0000000..e24ecce Binary files /dev/null and b/src/grr_description/meshes/brain.stl differ diff --git a/src/grr_description/meshes/bridge.stl b/src/grr_description/meshes/bridge.stl new file mode 100644 index 0000000..fed3acc Binary files /dev/null and b/src/grr_description/meshes/bridge.stl differ diff --git a/src/grr_description/meshes/bridge_holder.stl b/src/grr_description/meshes/bridge_holder.stl new file mode 100644 index 0000000..c4f6c1b Binary files /dev/null and b/src/grr_description/meshes/bridge_holder.stl differ diff --git a/src/grr_description/meshes/bridge_latch.stl b/src/grr_description/meshes/bridge_latch.stl new file mode 100644 index 0000000..be502e7 Binary files /dev/null and b/src/grr_description/meshes/bridge_latch.stl differ diff --git a/src/grr_description/meshes/camera.stl b/src/grr_description/meshes/camera.stl new file mode 100644 index 0000000..8006e94 Binary files /dev/null and b/src/grr_description/meshes/camera.stl differ diff --git a/src/grr_description/meshes/camera_stand.stl b/src/grr_description/meshes/camera_stand.stl new file mode 100644 index 0000000..85e0c6e Binary files /dev/null and b/src/grr_description/meshes/camera_stand.stl differ diff --git a/src/grr_description/meshes/color_sensor.stl b/src/grr_description/meshes/color_sensor.stl new file mode 100644 index 0000000..fc7594d Binary files /dev/null and b/src/grr_description/meshes/color_sensor.stl differ diff --git a/src/grr_description/meshes/measurements.txt b/src/grr_description/meshes/measurements.txt new file mode 100755 index 0000000..e428871 --- /dev/null +++ b/src/grr_description/meshes/measurements.txt @@ -0,0 +1,13 @@ +Chassis +Length: 180mm +Width: 330mm +Height: 135mm + +Joint Position +Toward Front: 145mm +Toward Side: 90mm +Toward bottom: 121 + +Wheel +Thickness: 42mm +Diameter ~: 46.575mm \ No newline at end of file diff --git a/src/grr_description/meshes/mechanum_roller.stl b/src/grr_description/meshes/mechanum_roller.stl new file mode 100644 index 0000000..96a2d36 Binary files /dev/null and b/src/grr_description/meshes/mechanum_roller.stl differ diff --git a/src/grr_description/meshes/mechanum_wheel_1.stl b/src/grr_description/meshes/mechanum_wheel_1.stl new file mode 100644 index 0000000..c0b87b8 Binary files /dev/null and b/src/grr_description/meshes/mechanum_wheel_1.stl differ diff --git a/src/grr_description/meshes/mechanum_wheel_2.stl b/src/grr_description/meshes/mechanum_wheel_2.stl new file mode 100644 index 0000000..e5e31ac Binary files /dev/null and b/src/grr_description/meshes/mechanum_wheel_2.stl differ diff --git a/src/grr_description/meshes/meshes.stl b/src/grr_description/meshes/meshes.stl new file mode 100644 index 0000000..c4f6c1b Binary files /dev/null and b/src/grr_description/meshes/meshes.stl differ diff --git a/src/grr_description/meshes/slim_drivebase.stl b/src/grr_description/meshes/slim_drivebase.stl new file mode 100644 index 0000000..6822c9e Binary files /dev/null and b/src/grr_description/meshes/slim_drivebase.stl differ diff --git a/src/grr_description/meshes/small_package_grabber.stl b/src/grr_description/meshes/small_package_grabber.stl new file mode 100644 index 0000000..892f08a Binary files /dev/null and b/src/grr_description/meshes/small_package_grabber.stl differ diff --git a/src/grr_description/meshes/small_package_grabber_roller.stl b/src/grr_description/meshes/small_package_grabber_roller.stl new file mode 100644 index 0000000..01febfa Binary files /dev/null and b/src/grr_description/meshes/small_package_grabber_roller.stl differ diff --git a/src/grr_description/meshes/small_package_grabber_tube.stl b/src/grr_description/meshes/small_package_grabber_tube.stl new file mode 100644 index 0000000..ee30c6b Binary files /dev/null and b/src/grr_description/meshes/small_package_grabber_tube.stl differ diff --git a/src/grr_description/meshes/small_package_sweeper.stl b/src/grr_description/meshes/small_package_sweeper.stl new file mode 100644 index 0000000..aa509dc Binary files /dev/null and b/src/grr_description/meshes/small_package_sweeper.stl differ diff --git a/src/grr_description/meshes/small_package_sweeper_roller.stl b/src/grr_description/meshes/small_package_sweeper_roller.stl new file mode 100644 index 0000000..c2c62f8 Binary files /dev/null and b/src/grr_description/meshes/small_package_sweeper_roller.stl differ diff --git a/src/grr_description/meshes/small_package_sweeper_servo.stl b/src/grr_description/meshes/small_package_sweeper_servo.stl new file mode 100644 index 0000000..6cabe0f Binary files /dev/null and b/src/grr_description/meshes/small_package_sweeper_servo.stl differ diff --git a/src/grr_description/meshes/zed2i.stl b/src/grr_description/meshes/zed2i.stl new file mode 100755 index 0000000..76a08d0 Binary files /dev/null and b/src/grr_description/meshes/zed2i.stl differ diff --git a/src/grr_description/package.xml b/src/grr_description/package.xml old mode 100644 new mode 100755 index 831677a..2580197 --- a/src/grr_description/package.xml +++ b/src/grr_description/package.xml @@ -3,12 +3,26 @@ grr_description 0.0.0 - TODO: Package description - philip - TODO: License declaration + Robot description test + helios + Apache License 2.0 ament_cmake + + xacro + ros2launch + robot_state_publisher + joint_state_broadcaster + joint_state_publisher_gui + ros2_control + ros2_controllers + joy + teleop_twist_joy + rviz2 + control_msgs + hardware_interface + grr_hardware ament_lint_auto ament_lint_common diff --git a/src/grr_description/urdf/control/drive.xacro b/src/grr_description/urdf/control/drive.xacro new file mode 100755 index 0000000..6bccea0 --- /dev/null +++ b/src/grr_description/urdf/control/drive.xacro @@ -0,0 +1,44 @@ + + + + + + grr_hardware/IsaacDriveHardware + 3.0 + 3.0 + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/bridge.xacro b/src/grr_description/urdf/parts/bridge.xacro new file mode 100644 index 0000000..66caf7e --- /dev/null +++ b/src/grr_description/urdf/parts/bridge.xacro @@ -0,0 +1,186 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/chassis.xacro b/src/grr_description/urdf/parts/chassis.xacro new file mode 100755 index 0000000..ed67d3c --- /dev/null +++ b/src/grr_description/urdf/parts/chassis.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/large_package_grabber.xacro b/src/grr_description/urdf/parts/large_package_grabber.xacro new file mode 100644 index 0000000..e69de29 diff --git a/src/grr_description/urdf/parts/lidar.xacro b/src/grr_description/urdf/parts/lidar.xacro new file mode 100755 index 0000000..e0f761c --- /dev/null +++ b/src/grr_description/urdf/parts/lidar.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/mecanum_roller.xacro b/src/grr_description/urdf/parts/mecanum_roller.xacro new file mode 100755 index 0000000..91dcf8d --- /dev/null +++ b/src/grr_description/urdf/parts/mecanum_roller.xacro @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/mecanum_wheel.xacro b/src/grr_description/urdf/parts/mecanum_wheel.xacro new file mode 100755 index 0000000..400037f --- /dev/null +++ b/src/grr_description/urdf/parts/mecanum_wheel.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/grr_description/urdf/parts/small_package_grabber.xacro b/src/grr_description/urdf/parts/small_package_grabber.xacro new file mode 100644 index 0000000..d7f70a7 --- /dev/null +++ b/src/grr_description/urdf/parts/small_package_grabber.xacro @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/small_package_sweeper.xacro b/src/grr_description/urdf/parts/small_package_sweeper.xacro new file mode 100644 index 0000000..ecdeb78 --- /dev/null +++ b/src/grr_description/urdf/parts/small_package_sweeper.xacro @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/parts/zed_cam.xacro b/src/grr_description/urdf/parts/zed_cam.xacro new file mode 100755 index 0000000..3b0d00b --- /dev/null +++ b/src/grr_description/urdf/parts/zed_cam.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/robots/bloodstone.urdf.xacro b/src/grr_description/urdf/robots/bloodstone.urdf.xacro new file mode 100755 index 0000000..d0fe8db --- /dev/null +++ b/src/grr_description/urdf/robots/bloodstone.urdf.xacro @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/grr_description/urdf/util/macros.xacro b/src/grr_description/urdf/util/macros.xacro new file mode 100755 index 0000000..5cf1cac --- /dev/null +++ b/src/grr_description/urdf/util/macros.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/grr_description/urdf/util/materials.xacro b/src/grr_description/urdf/util/materials.xacro new file mode 100755 index 0000000..6dd6af0 --- /dev/null +++ b/src/grr_description/urdf/util/materials.xacro @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/grr_hardware/CMakeLists.txt b/src/grr_hardware/CMakeLists.txt old mode 100644 new mode 100755 index d89c49e..8939116 --- a/src/grr_hardware/CMakeLists.txt +++ b/src/grr_hardware/CMakeLists.txt @@ -1,26 +1,78 @@ cmake_minimum_required(VERSION 3.8) project(grr_hardware) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(realtime_tools REQUIRED) + +## COMPILE +add_library( + ${PROJECT_NAME} + SHARED + src/isaac_drive.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) +ament_target_dependencies( + ${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs + realtime_tools +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml) + +# INSTALL +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) +install( + DIRECTORY include/ + DESTINATION include +) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) endif() +## EXPORTS +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + sensor_msgs +) + ament_package() diff --git a/src/grr_hardware/include/grr_hardware/isaac_drive.hpp b/src/grr_hardware/include/grr_hardware/isaac_drive.hpp new file mode 100755 index 0000000..f4cbba9 --- /dev/null +++ b/src/grr_hardware/include/grr_hardware/isaac_drive.hpp @@ -0,0 +1,93 @@ +// Copyright 2021 ros2_control Development Team +// +// 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. + +#ifndef GRR_HARDWARE__ISAAC_DRIVE_HPP_ +#define GRR_HARDWARE__ISAAC_DRIVE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "grr_hardware/visibility_control.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +// using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace grr_hardware +{ +class IsaacDriveHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(IsaacDriveHardware) + + GRR_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + GRR_HARDWARE_PUBLIC + std::vector export_state_interfaces() override; + + GRR_HARDWARE_PUBLIC + std::vector export_command_interfaces() override; + + GRR_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GRR_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GRR_HARDWARE_PUBLIC + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRR_HARDWARE_PUBLIC + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the DiffBot simulation + double hw_start_sec_; + double hw_stop_sec_; + + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_positions_; + std::vector hw_velocities_; + std::vector joint_names_; + std::map joint_names_map_; + + + // Pub Sub to isaac + rclcpp::Node::SharedPtr node_; + std::shared_ptr> isaac_publisher_ = nullptr; + std::shared_ptr> + realtime_isaac_publisher_ = nullptr; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr isaac_subscriber_ = nullptr; + realtime_tools::RealtimeBox> received_joint_msg_ptr_{nullptr}; +}; + +} // namespace grr_hardware + +#endif // GRR_HARDWARE__DIFFBOT_SYSTEM_HPP_ \ No newline at end of file diff --git a/src/grr_hardware/include/grr_hardware/visibility_control.h b/src/grr_hardware/include/grr_hardware/visibility_control.h new file mode 100755 index 0000000..25eaebf --- /dev/null +++ b/src/grr_hardware/include/grr_hardware/visibility_control.h @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef GRR_HARDWARE__VISIBILITY_CONTROL_H_ +#define GRR_HARDWARE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRR_HARDWARE_EXPORT __attribute__((dllexport)) +#define GRR_HARDWARE_IMPORT __attribute__((dllimport)) +#else +#define GRR_HARDWARE_EXPORT __declspec(dllexport) +#define GRR_HARDWARE_IMPORT __declspec(dllimport) +#endif +#ifdef GRR_HARDWARE_BUILDING_DLL +#define GRR_HARDWARE_PUBLIC GRR_HARDWARE_EXPORT +#else +#define GRR_HARDWARE_PUBLIC GRR_HARDWARE_IMPORT +#endif +#define GRR_HARDWARE_PUBLIC_TYPE GRR_HARDWARE_PUBLIC +#define GRR_HARDWARE_LOCAL +#else +#define GRR_HARDWARE_EXPORT __attribute__((visibility("default"))) +#define GRR_HARDWARE_IMPORT +#if __GNUC__ >= 4 +#define GRR_HARDWARE_PUBLIC __attribute__((visibility("default"))) +#define GRR_HARDWARE_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRR_HARDWARE_PUBLIC +#define GRR_HARDWARE_LOCAL +#endif +#define GRR_HARDWARE_PUBLIC_TYPE +#endif + +#endif // GRR_HARDWARE__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/src/grr_hardware/package.xml b/src/grr_hardware/package.xml old mode 100644 new mode 100755 index b561f68..3c8264a --- a/src/grr_hardware/package.xml +++ b/src/grr_hardware/package.xml @@ -8,6 +8,15 @@ TODO: License declaration ament_cmake + rclcpp + std_msgs + sensor_msgs + hardware_interface + pluginlib + rclcpp_lifecycle + realtime_tools + + ament_lint_auto ament_lint_common diff --git a/src/grr_hardware/robot_hardware.xml b/src/grr_hardware/robot_hardware.xml new file mode 100755 index 0000000..9ba9515 --- /dev/null +++ b/src/grr_hardware/robot_hardware.xml @@ -0,0 +1,9 @@ + + + + The ROS2 Control hardware interface to talk with Isaac Sim robot drive train. + + + \ No newline at end of file diff --git a/src/grr_hardware/src/isaac_drive.cpp b/src/grr_hardware/src/isaac_drive.cpp new file mode 100755 index 0000000..700e318 --- /dev/null +++ b/src/grr_hardware/src/isaac_drive.cpp @@ -0,0 +1,245 @@ +// Copyright 2021 ros2_control Development Team +// +// 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. + +#include "grr_hardware/isaac_drive.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +using std::placeholders::_1; + +namespace grr_hardware +{ +hardware_interface::CallbackReturn IsaacDriveHardware::on_init(const hardware_interface::HardwareInfo & info) +{ + + // rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + // custom_qos_profile.depth = 7; + + node_ = rclcpp::Node::make_shared("isaac_hardware_interface"); + + // PUBLISHER SETUP + isaac_publisher_ = node_->create_publisher("drive_command", rclcpp::SystemDefaultsQoS()); + realtime_isaac_publisher_ = std::make_shared>( + isaac_publisher_); + + // SUBSCRIBER SETUP + const sensor_msgs::msg::JointState empty_joint_state; + received_joint_msg_ptr_.set(std::make_shared(empty_joint_state)); + isaac_subscriber_ = node_->create_subscription("drive_state", rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { + RCLCPP_WARN( rclcpp::get_logger("isaac_hardware_interface"), "Can't accept new commands. subscriber is inactive"); + return; + } + received_joint_msg_ptr_.set(std::move(msg)); + }); + + + // INTERFACE SETUP + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + // hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + joint_names_.push_back(joint.name); + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("IsaacDriveHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("IsaacDriveHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("IsaacDriveHardware"), + "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("IsaacDriveHardware"), + "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + rclcpp::get_logger("IsaacDriveHardware"), + "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + } + + return CallbackReturn::SUCCESS; +} + + + +std::vector IsaacDriveHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + } + + return state_interfaces; +} + + + +std::vector IsaacDriveHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + } + + return command_interfaces; +} + + + +hardware_interface::CallbackReturn IsaacDriveHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("IsaacDriveHardware"), "Activating ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_positions_.size(); i++) + { + if (std::isnan(hw_positions_[i])) + { + hw_positions_[i] = 0; + hw_velocities_[i] = 0; + hw_commands_[i] = 0; + } + joint_names_map_[joint_names_[i]] = i + 1; // ADD 1 to differentiate null key + } + + subscriber_is_active_ = true; + + RCLCPP_INFO(rclcpp::get_logger("IsaacDriveHardware"), "Successfully activated!"); + + return CallbackReturn::SUCCESS; +} + + + +hardware_interface::CallbackReturn IsaacDriveHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("IsaacDriveHardware"), "Deactivating ...please wait..."); + subscriber_is_active_ = false; + RCLCPP_INFO(rclcpp::get_logger("IsaacDriveHardware"), "Successfully deactivated!"); + + return CallbackReturn::SUCCESS; +} + + +// || || +// \/ THE STUFF THAT MATTERS \/ + +hardware_interface::return_type grr_hardware::IsaacDriveHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + rclcpp::spin_some(node_); + std::shared_ptr last_command_msg; + received_joint_msg_ptr_.get(last_command_msg); + + if (last_command_msg == nullptr) + { + RCLCPP_WARN(rclcpp::get_logger("IsaacDriveHardware"), "Velocity message received was a nullptr."); + return hardware_interface::return_type::ERROR; + } + + auto names = last_command_msg->name; + auto positions = last_command_msg->position; + auto velocities = last_command_msg->velocity; + + for (auto i = 0u; i < names.size(); i++) { + uint p = joint_names_map_[names[i]]; + if (p > 0) { + hw_positions_[p - 1] = positions[i]; + hw_velocities_[p - 1] = velocities[i]; + } + } + + return hardware_interface::return_type::OK; +} + + + +hardware_interface::return_type grr_hardware::IsaacDriveHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // RCLCPP_INFO(rclcpp::get_logger("IsaacDriveHardware"), "Velocity: %f", hw_commands_[0]); + + if (realtime_isaac_publisher_->trylock()) { + auto & realtime_isaac_command = realtime_isaac_publisher_->msg_; + realtime_isaac_command.header.stamp = node_->get_clock()->now(); + realtime_isaac_command.name = joint_names_; + realtime_isaac_command.velocity = hw_commands_; + realtime_isaac_publisher_->unlockAndPublish(); + } + + rclcpp::spin_some(node_); + + return hardware_interface::return_type::OK; +} + +} // namespace grr_hardware + + + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + grr_hardware::IsaacDriveHardware, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/grr_hardware_python/grr_hardware_python/__init__.py b/src/grr_hardware_python/grr_hardware_python/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/grr_hardware_python/grr_hardware_python/robot.py b/src/grr_hardware_python/grr_hardware_python/robot.py new file mode 100644 index 0000000..4116575 --- /dev/null +++ b/src/grr_hardware_python/grr_hardware_python/robot.py @@ -0,0 +1,167 @@ +from typing import List +import rclpy +from rclpy.node import Node +from board import SCL, SDA +from busio import I2C + +from adafruit_pca9685 import PCA9685 +from adafruit_tca9548a import TCA9548A +from adafruit_tcs34725 import TCS34725 + + +from sensor_msgs.msg import JointState +from std_msgs.msg import Int64MultiArray, Bool + +from rcl_interfaces.msg import SetParametersResult + +import math +import time +from busio import I2C + +class Line_Follower(object): + def __init__(self, i2c:I2C, address=0x11, references=[300, 300, 300, 300, 300]): + self.i2c = i2c + self.address = address + self._references = references + + def read_raw(self): + for i in range(0, 5): + try: + raw_result = bytearray(10) + self.i2c.readfrom_into(self.address, raw_result, end=10) + Connection_OK = True + break + except Exception as e: + print(e) + Connection_OK = False + + if Connection_OK: + return raw_result + else: + print("Error accessing %2X" % self.address) + return False + + def read_analog(self, trys=5): + for _ in range(trys): + raw_result = self.read_raw() + if raw_result: + analog_result = [0, 0, 0, 0, 0] + for i in range(0, 5): + high_byte = raw_result[i*2] << 8 + low_byte = raw_result[i*2+1] + analog_result[i] = high_byte + low_byte + if analog_result[i] > 1024: + continue + return analog_result + else: + raise IOError("Line follower read error. Please check the wiring.") + +class Robot(Node): + def __init__(self) -> None: + super().__init__("robot") + self.i2c = I2C(SCL, SDA) + + self.tca = TCA9548A(self.i2c) + + self.start_light_timer = self.create_timer(.1, self.start_light_checker) + self.back_sensor = TCS34725(self.tca[1]) + + self.start_light_publisher = self.create_publisher(Bool, "/grr/start_light", 10) + self.declare_parameter('LED_Threshold', 100) + + self.line_follower = Line_Follower(self.i2c) + self.line_timer = self.create_timer(1/30, self.line_array) + self.line_publisher = self.create_publisher(Int64MultiArray, '/lineArray', 10) + + self.servo_joint_names = ["bridge_latch_joint", "mechanism_lift_joint", "mechanism_package_joint", "mechanism_thruster_joint", "small_package_sweeper_joint"] + params = [ + ('pca_address', 0x40), + ('pca_frequency', 50) + ] + self.servo_mapping = {} + + for joint_name in self.servo_joint_names: + params.append((f'servo_mapping.{joint_name}.port', 15)) + params.append((f'servo_mapping.{joint_name}.maximum', 1)) + params.append((f'servo_mapping.{joint_name}.minimum', 0)) + self.servo_mapping[joint_name] = {} + + self.declare_parameters( + namespace='', + parameters=params + ) + + self.PCA_ADDY = 0 + self.PCA_FREQ = 0 + self.load_parameters() + self.connect_to_pca(self.i2c) + + self.get_logger().info(f'servo mapping: {self.servo_mapping}') + + self.add_on_set_parameters_callback(self.parameters_callback) + + self.joint_state_subscriber = self.create_subscription(JointState, "/grr/joint_command", self.joint_command_callback, 10) + + def parameters_callback(self, params): + self.get_logger().info(f"{params}") + self.load_parameters() + return SetParametersResult(successful=True) + + def load_parameters(self): + self.PCA_ADDY = self.get_parameter('pca_address').get_parameter_value().integer_value + self.PCA_FREQ = self.get_parameter('pca_frequency').get_parameter_value().integer_value + self.get_logger().info(f"ADDY: {self.PCA_ADDY} FREQ: {self.PCA_FREQ}") + for joint_name in self.servo_joint_names: + self.servo_mapping[joint_name]['port'] = self.get_parameter(f'servo_mapping.{joint_name}.port').get_parameter_value().integer_value + self.servo_mapping[joint_name]['maximum'] = self.get_parameter(f'servo_mapping.{joint_name}.maximum').get_parameter_value().integer_value + self.servo_mapping[joint_name]['minimum'] = self.get_parameter(f'servo_mapping.{joint_name}.minimum').get_parameter_value().integer_value + + def connect_to_pca(self, i2c:I2C): + self.pca = PCA9685(i2c, address=self.PCA_ADDY) + self.pca.frequency = self.PCA_FREQ + + def joint_command_callback(self, data:JointState): + for i, name in enumerate(data.name): + self.get_logger().info(f'link name: {name} VS stored: {self.servo_mapping.keys()}') + if name not in self.servo_mapping.keys(): + self.get_logger().info('Skipped') + continue + effort = data.effort[i] + old_min = 0 + old_max = 100 + old_range = old_max - old_min + new_min = self.servo_mapping[name]['minimum'] + new_max = self.servo_mapping[name]['maximum'] + new_range = new_max-new_min + + duty_cycle = (((effort - old_min) * new_range)/old_range)+new_min + + self.get_logger().info(f"Spinning servo: {self.servo_mapping[name]['port']} to duty cycle: {duty_cycle} at frequency {self.pca.frequency}") + + self.pca.channels[self.servo_mapping[name]['port']].duty_cycle = int(duty_cycle) + + def line_array(self): + vals = self.line_follower.read_analog() + self.line_publisher.publish(Int64MultiArray(data=vals)) + + def start_light_checker(self): + threshold = self.get_parameter('LED_Threshold').get_parameter_value().integer_value + color_rgb = self.back_sensor.color_rgb_bytes + msg = Bool() + msg.data = color_rgb[0] >= threshold + self.start_light_publisher.publish(msg) + """ if msg.data: + self.start_light_timer.destroy() """ + + + +def main(): + rclpy.init() + robot = Robot() + rclpy.spin(robot) + robot.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/grr_hardware_python/package.xml b/src/grr_hardware_python/package.xml new file mode 100644 index 0000000..02b306e --- /dev/null +++ b/src/grr_hardware_python/package.xml @@ -0,0 +1,18 @@ + + + + grr_hardware_python + 0.0.0 + TODO: Package description + grr + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/grr_hardware_python/resource/grr_hardware_python b/src/grr_hardware_python/resource/grr_hardware_python new file mode 100644 index 0000000..e69de29 diff --git a/src/grr_hardware_python/setup.cfg b/src/grr_hardware_python/setup.cfg new file mode 100644 index 0000000..5ef9d9f --- /dev/null +++ b/src/grr_hardware_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/grr_hardware_python +[install] +install_scripts=$base/lib/grr_hardware_python diff --git a/src/grr_hardware_python/setup.py b/src/grr_hardware_python/setup.py new file mode 100644 index 0000000..e25d69f --- /dev/null +++ b/src/grr_hardware_python/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'grr_hardware_python' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='grr', + maintainer_email='grr@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'robot = grr_hardware_python.robot:main' + ], + }, +) diff --git a/src/grr_hardware_python/test/test_copyright.py b/src/grr_hardware_python/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/grr_hardware_python/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/grr_hardware_python/test/test_flake8.py b/src/grr_hardware_python/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/grr_hardware_python/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) diff --git a/src/grr_hardware_python/test/test_pep257.py b/src/grr_hardware_python/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/grr_hardware_python/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' diff --git a/src/grr_interfaces/CMakeLists.txt b/src/grr_interfaces/CMakeLists.txt old mode 100644 new mode 100755 index d8a1c62..2465ece --- a/src/grr_interfaces/CMakeLists.txt +++ b/src/grr_interfaces/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -23,4 +24,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +#rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/LineArray.msg" +# ) + + ament_package() diff --git a/src/grr_interfaces/package.xml b/src/grr_interfaces/package.xml old mode 100644 new mode 100755 index 21f7b74..e5b8626 --- a/src/grr_interfaces/package.xml +++ b/src/grr_interfaces/package.xml @@ -9,6 +9,13 @@ ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto ament_lint_common diff --git a/src/grr_python_controllers/grr_python_controllers/__init__.py b/src/grr_python_controllers/grr_python_controllers/__init__.py old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/package.xml b/src/grr_python_controllers/package.xml old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/resource/grr_python_controllers b/src/grr_python_controllers/resource/grr_python_controllers old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/setup.cfg b/src/grr_python_controllers/setup.cfg old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/setup.py b/src/grr_python_controllers/setup.py old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/test/test_copyright.py b/src/grr_python_controllers/test/test_copyright.py old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/test/test_flake8.py b/src/grr_python_controllers/test/test_flake8.py old mode 100644 new mode 100755 diff --git a/src/grr_python_controllers/test/test_pep257.py b/src/grr_python_controllers/test/test_pep257.py old mode 100644 new mode 100755 diff --git a/src/grr_test/grr_test/__init__.py b/src/grr_test/grr_test/__init__.py old mode 100644 new mode 100755 diff --git a/src/grr_test/package.xml b/src/grr_test/package.xml old mode 100644 new mode 100755 diff --git a/src/grr_test/resource/grr_test b/src/grr_test/resource/grr_test old mode 100644 new mode 100755 diff --git a/src/grr_test/setup.cfg b/src/grr_test/setup.cfg old mode 100644 new mode 100755 diff --git a/src/grr_test/setup.py b/src/grr_test/setup.py old mode 100644 new mode 100755 diff --git a/src/grr_test/test/test_copyright.py b/src/grr_test/test/test_copyright.py old mode 100644 new mode 100755 diff --git a/src/grr_test/test/test_flake8.py b/src/grr_test/test/test_flake8.py old mode 100644 new mode 100755 diff --git a/src/grr_test/test/test_pep257.py b/src/grr_test/test/test_pep257.py old mode 100644 new mode 100755