diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index e393e89..9f178b6 100644 --- a/README.md +++ b/README.md @@ -2,4 +2,4 @@ This repository provides examples for running robots using Space ROS -Please refer to the [dockerfile repo](https://github.com/space-ros/docker-images/tree/main/demo_spaceros) for running instruction +Please refer to the [dockerfile repo](https://github.com/space-ros/docker/tree/main/space_robots) for running instruction diff --git a/canadarm/CMakeLists.txt b/canadarm/CMakeLists.txt index f782dc4..80899a7 100644 --- a/canadarm/CMakeLists.txt +++ b/canadarm/CMakeLists.txt @@ -43,6 +43,7 @@ install(DIRECTORY install(PROGRAMS nodes/move_joint_server + nodes/move_arm DESTINATION lib/${PROJECT_NAME} ) diff --git a/canadarm/README.md b/canadarm/README.md index 97a4f74..627913c 100644 --- a/canadarm/README.md +++ b/canadarm/README.md @@ -3,8 +3,26 @@ (This is a temp instruction before the reorg) -Build and source the ```canadarm``` package +Build and source the ```canadarm``` package: ``` ros2 launch canadarm canadarm.launch.py -``` \ No newline at end of file +``` + +To move the arm to an outstretched pose: + +``` +ros2 service call /open_arm std_srvs/srv/Empty +``` + +To move the arm to its default (close) pose of all zeros: + +``` +ros2 service call /close_arm std_srvs/srv/Empty +``` + +To move the arm to a random pose: + +``` +ros2 service call /random_arm std_srvs/srv/Empty +``` diff --git a/canadarm/config/canadarm_control.yaml b/canadarm/config/canadarm_control.yaml index 1677d36..ce12266 100644 --- a/canadarm/config/canadarm_control.yaml +++ b/canadarm/config/canadarm_control.yaml @@ -5,10 +5,30 @@ controller_manager: canadarm_joint_controller: type: effort_controllers/JointGroupPositionController + canadarm_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster +canadarm_joint_trajectory_controller: + ros__parameters: + joints: + - Base_Joint + - Shoulder_Roll + - Shoulder_Yaw + - Elbow_Pitch + - Wrist_Pitch + - Wrist_Yaw + - Wrist_Roll + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + canadarm_joint_controller: ros__parameters: joints: @@ -53,4 +73,4 @@ canadarm_joint_controller: Wrist_Roll: p: 50.0 i: 10.0 - d: 200.0 \ No newline at end of file + d: 200.0 diff --git a/canadarm/launch/canadarm.launch.py b/canadarm/launch/canadarm.launch.py index b6d17ce..91a843f 100644 --- a/canadarm/launch/canadarm.launch.py +++ b/canadarm/launch/canadarm.launch.py @@ -23,23 +23,30 @@ def generate_launch_description(): ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), environ.get('LD_LIBRARY_PATH', default='')]), 'IGN_GAZEBO_RESOURCE_PATH': - ':'.join([canadarm_demos_path])} + ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), canadarm_demos_path])} - urdf_model_path = os.path.join(simulation_models_path, 'models', 'canadarm', 'urdf', 'SSRMS_Canadarm2.urdf') + urdf_model_path = os.path.join(simulation_models_path, 'models', 'canadarm', 'urdf', 'SSRMS_Canadarm2.urdf.xacro') leo_model = os.path.join(canadarm_demos_path, 'worlds', 'simple.world') - doc = xacro.process_file(urdf_model_path) + doc = xacro.process_file(urdf_model_path, mappings={'xyz' : '1.0 0.0 1.5', 'rpy': '3.1416 0.0 0.0'}) robot_description = {'robot_description': doc.toxml()} - run_node = Node( + #run_node = Node( + # package="canadarm", + # executable="move_joint_server", + # output='screen' + #) + + run_move_arm = Node( package="canadarm", - executable="move_joint_server", + executable="move_arm", output='screen' ) + start_world = ExecuteProcess( cmd=['ign gazebo', leo_model, '-r'], output='screen', @@ -74,7 +81,7 @@ def generate_launch_description(): load_canadarm_joint_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'canadarm_joint_controller'], + 'canadarm_joint_trajectory_controller'], output='screen' ) @@ -84,7 +91,8 @@ def generate_launch_description(): start_world, robot_state_publisher, spawn, - run_node, + #run_node, + run_move_arm, RegisterEventHandler( OnProcessExit( @@ -98,4 +106,4 @@ def generate_launch_description(): on_exit=[load_canadarm_joint_controller], ) ), - ]) \ No newline at end of file + ]) diff --git a/canadarm/nodes/hello_moveit.cpp b/canadarm/nodes/hello_moveit.cpp new file mode 100644 index 0000000..9e3f190 --- /dev/null +++ b/canadarm/nodes/hello_moveit.cpp @@ -0,0 +1,51 @@ +#include + +#include +#include + +int main(int argc, char* argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "canadarm"); + + // Set a target Pose + auto const target_pose = [] { + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.28; + msg.position.y = -0.2; + msg.position.z = 0.5; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + move_group_interface.execute(plan); + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + } + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/canadarm/nodes/move_arm b/canadarm/nodes/move_arm new file mode 100755 index 0000000..edc1f83 --- /dev/null +++ b/canadarm/nodes/move_arm @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import String, Float64 +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_srvs.srv import Empty + +class MoveArm(Node): + + def __init__(self): + super().__init__('arm_node') + self.arm_publisher_ = self.create_publisher(JointTrajectory, '/canadarm_joint_trajectory_controller/joint_trajectory', 10) + self.open_srv = self.create_service(Empty, 'open_arm', self.open_arm_callback) + self.close_srv = self.create_service(Empty, 'close_arm', self.close_arm_callback) + self.random_srv = self.create_service(Empty, 'random_arm', self.random_arm_callback) + + + def open_arm_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"] + + point1 = JointTrajectoryPoint() + point1.positions = [0.0, 0.0, 0.0, -3.1416, 0.0, 0.0, 0.0] + point1.time_from_start = Duration(sec=4) + + traj.points.append(point1) + self.arm_publisher_.publish(traj) + + + return response + + def close_arm_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"] + + point1 = JointTrajectoryPoint() + point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + point1.time_from_start = Duration(sec=4) + + traj.points.append(point1) + self.arm_publisher_.publish(traj) + + return response + + def random_arm_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"] + + point1 = JointTrajectoryPoint() + point1.positions = [1.0, -1.5, 2.0, -3.2, 0.8, 0.5, -1.0] + point1.time_from_start = Duration(sec=4) + + traj.points.append(point1) + self.arm_publisher_.publish(traj) + + return response + + +def main(args=None): + rclpy.init(args=args) + + arm_node = MoveArm() + + rclpy.spin(arm_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + arm_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/canadarm/nodes/move_joint_server b/canadarm/nodes/move_joint_server old mode 100644 new mode 100755 diff --git a/canadarm/worlds/simple.world b/canadarm/worlds/simple.world index b068641..6441760 100644 --- a/canadarm/worlds/simple.world +++ b/canadarm/worlds/simple.world @@ -47,7 +47,7 @@ - package://simulation/canadarm/meshes/earth.dae + model://canadarm/meshes/earth.dae 3 3 3 @@ -77,7 +77,7 @@ - package://simulation/canadarm/meshes/iss.dae + model://canadarm/meshes/iss.dae 1 1 1 @@ -92,4 +92,4 @@ - \ No newline at end of file + diff --git a/canadarm_moveit_config/.setup_assistant b/canadarm_moveit_config/.setup_assistant new file mode 100644 index 0000000..23da2f9 --- /dev/null +++ b/canadarm_moveit_config/.setup_assistant @@ -0,0 +1,27 @@ +moveit_setup_assistant_config: + urdf: + package: simulation + relative_path: models/canadarm/urdf/SSRMS_Canadarm2.urdf + srdf: + relative_path: config/SSRMS_Canadarm2.srdf + package_settings: + author_name: Dharini Dutia + author_email: dharini@openrobotics.org + generated_timestamp: 1672757808 + control_xacro: + command: + - position + - velocity + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + - velocity + state: + - position + - velocity \ No newline at end of file diff --git a/canadarm_moveit_config/CMakeLists.txt b/canadarm_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..892e977 --- /dev/null +++ b/canadarm_moveit_config/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(canadarm_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro b/canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro new file mode 100644 index 0000000..fa801a7 --- /dev/null +++ b/canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro @@ -0,0 +1,70 @@ + + + + + + + + + mock_components/GenericSystem + + + + + + ${initial_positions['Base_Joint']} + + + + + + + + ${initial_positions['Shoulder_Roll']} + + + + + + + + ${initial_positions['Shoulder_Yaw']} + + + + + + + + ${initial_positions['Elbow_Pitch']} + + + + + + + + ${initial_positions['Wrist_Pitch']} + + + + + + + + ${initial_positions['Wrist_Yaw']} + + + + + + + + ${initial_positions['Wrist_Roll']} + + + + + + + diff --git a/canadarm_moveit_config/config/SSRMS_Canadarm2.srdf b/canadarm_moveit_config/config/SSRMS_Canadarm2.srdf new file mode 100644 index 0000000..3b6cf26 --- /dev/null +++ b/canadarm_moveit_config/config/SSRMS_Canadarm2.srdf @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro b/canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro new file mode 100644 index 0000000..0fa43d5 --- /dev/null +++ b/canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/canadarm_moveit_config/config/initial_positions.yaml b/canadarm_moveit_config/config/initial_positions.yaml new file mode 100644 index 0000000..da70cfb --- /dev/null +++ b/canadarm_moveit_config/config/initial_positions.yaml @@ -0,0 +1,10 @@ +# Default initial positions for SSRMS_Canadarm2's ros2_control fake system + +initial_positions: + Base_Joint: 0 + Elbow_Pitch: 0 + Shoulder_Roll: 0 + Shoulder_Yaw: 0 + Wrist_Pitch: 0 + Wrist_Roll: 0 + Wrist_Yaw: 0 \ No newline at end of file diff --git a/canadarm_moveit_config/config/joint_limits.yaml b/canadarm_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..911a461 --- /dev/null +++ b/canadarm_moveit_config/config/joint_limits.yaml @@ -0,0 +1,45 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + Base_Joint: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Elbow_Pitch: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Shoulder_Roll: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Shoulder_Yaw: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Wrist_Pitch: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Wrist_Roll: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 + Wrist_Yaw: + has_velocity_limits: true + max_velocity: 0.069813200000000006 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/canadarm_moveit_config/config/kinematics.yaml b/canadarm_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..4f3692d --- /dev/null +++ b/canadarm_moveit_config/config/kinematics.yaml @@ -0,0 +1,8 @@ +canadarm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 +canadarm_planning_group: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/canadarm_moveit_config/config/moveit.rviz b/canadarm_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..f31651e --- /dev/null +++ b/canadarm_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/canadarm_moveit_config/config/moveit_controllers.yaml b/canadarm_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..b5e976a --- /dev/null +++ b/canadarm_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,20 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - canadarm_controller + + canadarm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - Base_Joint + - Shoulder_Roll + - Shoulder_Yaw + - Elbow_Pitch + - Wrist_Pitch + - Wrist_Yaw + - Wrist_Roll \ No newline at end of file diff --git a/canadarm_moveit_config/config/pilz_cartesian_limits.yaml b/canadarm_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/canadarm_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/canadarm_moveit_config/config/ros2_controllers.yaml b/canadarm_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..59d4841 --- /dev/null +++ b/canadarm_moveit_config/config/ros2_controllers.yaml @@ -0,0 +1,28 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + canadarm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +canadarm_controller: + ros__parameters: + joints: + - Base_Joint + - Shoulder_Roll + - Shoulder_Yaw + - Elbow_Pitch + - Wrist_Pitch + - Wrist_Yaw + - Wrist_Roll + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/canadarm_moveit_config/launch/demo.launch.py b/canadarm_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..861fc1d --- /dev/null +++ b/canadarm_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/move_group.launch.py b/canadarm_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..bd021f2 --- /dev/null +++ b/canadarm_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/moveit_rviz.launch.py b/canadarm_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..1bbc2c0 --- /dev/null +++ b/canadarm_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/rsp.launch.py b/canadarm_moveit_config/launch/rsp.launch.py new file mode 100644 index 0000000..174a11c --- /dev/null +++ b/canadarm_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/setup_assistant.launch.py b/canadarm_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..0cc0a78 --- /dev/null +++ b/canadarm_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/spawn_controllers.launch.py b/canadarm_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..039bfb4 --- /dev/null +++ b/canadarm_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py b/canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..439937a --- /dev/null +++ b/canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/canadarm_moveit_config/launch/warehouse_db.launch.py b/canadarm_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..41f2418 --- /dev/null +++ b/canadarm_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/canadarm_moveit_config/package.xml b/canadarm_moveit_config/package.xml new file mode 100644 index 0000000..a266ea5 --- /dev/null +++ b/canadarm_moveit_config/package.xml @@ -0,0 +1,52 @@ + + + + canadarm_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the SSRMS_Canadarm2 with the MoveIt Motion Planning Framework + + Dharini Dutia + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Dharini Dutia + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + simulation + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + + diff --git a/mars_rover/CMakeLists.txt b/mars_rover/CMakeLists.txt index e200664..c2c9005 100644 --- a/mars_rover/CMakeLists.txt +++ b/mars_rover/CMakeLists.txt @@ -45,6 +45,7 @@ install(PROGRAMS nodes/move_mast nodes/move_wheel nodes/run_demo + nodes/odom_tf_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py index e6ebd29..71ff515 100644 --- a/mars_rover/launch/mars_rover.launch.py +++ b/mars_rover/launch/mars_rover.launch.py @@ -2,7 +2,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.event_handlers import OnProcessExit, OnExecutionComplete @@ -36,7 +36,6 @@ def generate_launch_description(): doc = xacro.process_file(urdf_model_path) robot_description = {'robot_description': doc.toxml()} - arm_node = Node( package="mars_rover", executable="move_arm", @@ -61,6 +60,12 @@ def generate_launch_description(): output='screen' ) + odom_node = Node( + package="mars_rover", + executable="odom_tf_publisher", + output='screen' + ) + start_world = ExecuteProcess( cmd=['gz sim', mars_world_model, '-r'], output='screen', @@ -74,14 +79,32 @@ def generate_launch_description(): name='robot_state_publisher', output='screen', parameters=[robot_description]) + + ros_gz_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/model/curiosity_mars_rover/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', + ], + output='screen') + + image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/image_raw', '/image_raw'], + output='screen') spawn = Node( package='ros_gz_sim', executable='create', arguments=[ - '-name', 'curiosity_path', + '-name', 'curiosity_mars_rover', '-topic', robot_description, + '-z', '-7.5' ], output='screen' + ) @@ -134,6 +157,7 @@ def generate_launch_description(): ) return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), start_world, robot_state_publisher, spawn, @@ -141,6 +165,9 @@ def generate_launch_description(): mast_node, wheel_node, run_node, + odom_node, + ros_gz_bridge, + image_bridge, RegisterEventHandler( OnProcessExit( @@ -164,4 +191,4 @@ def generate_launch_description(): load_suspension_joint_traj_controller], ) ), - ]) \ No newline at end of file + ]) diff --git a/mars_rover/nodes/odom_tf_publisher b/mars_rover/nodes/odom_tf_publisher new file mode 100644 index 0000000..ca5c95f --- /dev/null +++ b/mars_rover/nodes/odom_tf_publisher @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +# adapted from ROS 2 tutorial +# https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html + +import rclpy + +from nav_msgs.msg import Odometry +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster + +class OdomTFBroadcaster(Node): + + def __init__(self): + super().__init__('odom_tf_publisher') + + # Initialize the transform broadcaster + self.tf_broadcaster = TransformBroadcaster(self) + + # Subscribe to Odometry topic + # TODO: make the topic name a parameter instead of hard-coded + self.subscription = self.create_subscription( + Odometry, + f'/model/curiosity_mars_rover/odometry', + self.handle_odometry, + 1) + self.subscription # prevent unused variable warning + + def handle_odometry(self, msg): + tf = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + tf.header.stamp = msg.header.stamp + tf.header.frame_id = msg.header.frame_id + tf.child_frame_id = msg.child_frame_id + + # get translation coordinates from the message pose + tf.transform.translation.x = msg.pose.pose.position.x + tf.transform.translation.y = msg.pose.pose.position.y + tf.transform.translation.z = msg.pose.pose.position.z + + # get rotation from the message pose + tf.transform.rotation.x = msg.pose.pose.orientation.x + tf.transform.rotation.y = msg.pose.pose.orientation.y + tf.transform.rotation.z = msg.pose.pose.orientation.z + tf.transform.rotation.w = msg.pose.pose.orientation.w + + # Send the transformation + self.tf_broadcaster.sendTransform(tf) + + +def main(): + rclpy.init() + node = OdomTFBroadcaster() + node.get_logger().info('Starting odometry_tf_publisher node') + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mars_rover/nodes/run_demo b/mars_rover/nodes/run_demo index 8dc6c18..6df8d17 100755 --- a/mars_rover/nodes/run_demo +++ b/mars_rover/nodes/run_demo @@ -20,6 +20,7 @@ class RunDemo(Node): self.stop_srv = self.create_service(Empty, 'move_stop', self.move_stop_callback) self.left_srv = self.create_service(Empty, 'turn_left', self.turn_left_callback) self.right_srv = self.create_service(Empty, 'turn_right', self.turn_right_callback) + self.stopped = True timer_period = 0.1 self.timer = self.create_timer(timer_period, self.timer_callback) @@ -27,34 +28,45 @@ class RunDemo(Node): self.curr_action = Twist() def timer_callback(self): - self.motion_publisher_.publish(self.curr_action) + if (not self.stopped): + self.motion_publisher_.publish(self.curr_action) def move_forward_callback(self, request, response): + self.get_logger().info("Moving forward") action = Twist() action.linear.x = 2.0 self.curr_action = action + self.stopped = False return response def move_stop_callback(self, request, response): + # stop timer from publishing + self.stopped = True + self.get_logger().info("Stopping") self.curr_action = Twist() + # publish once to ensure we stop + self.motion_publisher_.publish(self.curr_action) return response def turn_left_callback(self, request, response): + self.get_logger().info("Turning left") action = Twist() action.linear.x = 1.0 action.angular.z = 0.4 self.curr_action = action + self.stopped = False return response def turn_right_callback(self, request, response): + self.get_logger().info("Turning right") + self.stopped = False action = Twist() action.linear.x = 1.0 action.angular.z = -0.4 self.curr_action = action + self.stopped = False return response - - def main(args=None): rclpy.init(args=args) diff --git a/mars_rover/worlds/mars_curiosity.world b/mars_rover/worlds/mars_curiosity.world index 92e0cfa..375c2f9 100755 --- a/mars_rover/worlds/mars_curiosity.world +++ b/mars_rover/worlds/mars_curiosity.world @@ -240,13 +240,13 @@ - 0 0 -3.711 + 0 0 -3.711 - - /home/spaceros-user/demos_ws/install/simulation/share/simulation/models/curiosity_path - mars_terrain - 0 0 0 0 0 0 - + + model://curiosity_path + mars_terrain + 0 0 0 0 0 0 +