diff --git a/canadarm2/.defaults.yaml b/canadarm2/.defaults.yaml new file mode 100644 index 0000000..146c3c9 --- /dev/null +++ b/canadarm2/.defaults.yaml @@ -0,0 +1,7 @@ +{ + "build": + { + "symlink-install": true, + "cmake_args": "-DCMAKE_BUILD_TYPE=Release -Wno-dev", + }, +} diff --git a/canadarm2/Dockerfile b/canadarm2/Dockerfile new file mode 100644 index 0000000..dbbfe05 --- /dev/null +++ b/canadarm2/Dockerfile @@ -0,0 +1,23 @@ +FROM osrf/space-ros:core + +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +# Install dependencies +RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-control-msgs \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp + +# Prepare the workspace +SHELL ["bash", "-c"] +RUN mkdir -p ${HOME}/canadarm_ws/src +WORKDIR ${HOME}/canadarm_ws + +COPY ./canadarm_demo src/canadarm_demo +COPY .defaults.yaml .defaults.yaml + +# Build the workspace +RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ + && source /opt/spaceros/setup.bash \ + && colcon build + +# Source the workspace +RUN echo "source ${HOME}/canadarm_ws/install/setup.bash" >> ${HOME}/.bashrc diff --git a/canadarm2/Makefile b/canadarm2/Makefile new file mode 100644 index 0000000..cb95b69 --- /dev/null +++ b/canadarm2/Makefile @@ -0,0 +1,60 @@ +# Define variables +DOCKER_IMAGE = osrf/space-ros:canadarm_demo +WORKSPACE = ${HOME}/canadarm_ws +LOCAL_WORKSPACE = $(shell pwd) +SHELL := /bin/bash +XTERM_CONFIG = -bg black -fg white -fa 'Monospace' -fs 11 + +# Help target to describe each target +.PHONY: help +help: + @echo "CanadArm2 Makefile" + @echo " make build - Build the Docker image and the Gazebo workspace locally" + @echo " make build-docker - Build the Docker image" + @echo " make build-gazebo - Build the Gazebo workspace locally" + @echo " make run-gazebo - Run the CanadArm2 Gazebo simulation locally" + @echo " make run-demo - Run the CanadArm2 demo within Docker" + @echo " make clean - Clean the local workspace" + @echo " make all - Build and run everything" + +# Build all +.PHONY: build +build: build-docker build-gazebo + +# Build the Docker image +.PHONY: build-docker +build-docker: + docker build -t $(DOCKER_IMAGE) . + +# Run the CanadArm Gazebo simulation locally +.PHONY: run-gazebo +run-gazebo: + xterm $(XTERM_CONFIG) -T 'CanadArm2 Gazebo' -e 'source /opt/ros/${ROS_DISTRO}/setup.bash \ + && source $(LOCAL_WORKSPACE)/install/setup.bash \ + && ros2 launch canadarm_gazebo canadarm.launch.py' & + +# Build the Gazebo workspace locally +.PHONY: build-gazebo +build-gazebo: + @source /opt/ros/${ROS_DISTRO}/setup.bash && \ + rosdep install --from-paths canadarm_gazebo --ignore-src -r -y && \ + rosdep install --from-paths canadarm_description --ignore-src -r -y && \ + colcon build --symlink-install --base-paths $(LOCAL_WORKSPACE) --install-base $(LOCAL_WORKSPACE)/install \ + --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \ + --packages-select canadarm_description canadarm_gazebo + +# Run the CanadArm demo within Docker +.PHONY: run-docker +run-docker: + xterm $(XTERM_CONFIG) -T 'CanadArm2 Demo' -e "docker run -it --rm \ + -e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp \ + $(DOCKER_IMAGE) \ + bash -c 'source ~/.bashrc && ros2 launch canadarm_demo canadarm.launch.py'" & + +.PHONY: clean +clean: + rm -rf $(LOCAL_WORKSPACE)/install $(LOCAL_WORKSPACE)/log $(LOCAL_WORKSPACE)/build + +# Build and run everything +.PHONY: run +run: build run-gazebo run-docker diff --git a/canadarm2/canadarm_demo/CMakeLists.txt b/canadarm2/canadarm_demo/CMakeLists.txt index 1978016..14ed3ab 100644 --- a/canadarm2/canadarm_demo/CMakeLists.txt +++ b/canadarm2/canadarm_demo/CMakeLists.txt @@ -10,9 +10,6 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(rosidl_default_generators REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "action/MoveJoint.action" @@ -34,9 +31,7 @@ endif() install(DIRECTORY action - config launch - worlds DESTINATION share/${PROJECT_NAME} ) diff --git a/canadarm2/canadarm_demo/config/canadarm_control.yaml b/canadarm2/canadarm_demo/config/canadarm_control.yaml deleted file mode 100644 index ce12266..0000000 --- a/canadarm2/canadarm_demo/config/canadarm_control.yaml +++ /dev/null @@ -1,76 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 - - 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: - - Base_Joint - - Shoulder_Roll - - Shoulder_Yaw - - Elbow_Pitch - - Wrist_Pitch - - Wrist_Yaw - - Wrist_Roll - command_interfaces: - - effort - state_interfaces: - - position - - velocity - - effort - gains: - Base_Joint: - p: 50.0 - i: 100.0 - d: 2000.0 - Shoulder_Roll: - p: 50.0 - i: 100.0 - d: 2000.0 - Shoulder_Yaw: - p: 50.0 - i: 100.0 - d: 2000.0 - Elbow_Pitch: - p: 50.0 - i: 100.0 - d: 2000.0 - Wrist_Pitch: - p: 50.0 - i: 100.0 - d: 2000.0 - Wrist_Yaw: - p: 50.0 - i: 10.0 - d: 200.0 - Wrist_Roll: - p: 50.0 - i: 10.0 - d: 200.0 diff --git a/canadarm2/canadarm_demo/launch/canadarm.launch.py b/canadarm2/canadarm_demo/launch/canadarm.launch.py index 91a843f..825f67d 100644 --- a/canadarm2/canadarm_demo/launch/canadarm.launch.py +++ b/canadarm2/canadarm_demo/launch/canadarm.launch.py @@ -1,109 +1,24 @@ -from http.server import executable -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler -from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch.event_handlers import OnProcessExit, OnExecutionComplete -import os -from os import environ +"""Canadarm2 demo launch file.""" -from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription # type: ignore -import xacro +from launch_ros.actions import Node # type: ignore def generate_launch_description(): - # ld = LaunchDescription() + """Generate launch description with multiple components.""" - canadarm_demos_path = get_package_share_directory('canadarm') - simulation_models_path = get_package_share_directory('simulation') - - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_RESOURCE_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.xacro') - leo_model = os.path.join(canadarm_demos_path, 'worlds', 'simple.world') - - - 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_arm", - output='screen' - ) - - - start_world = ExecuteProcess( - cmd=['ign gazebo', leo_model, '-r'], - output='screen', - additional_env=env, - shell=True - ) - - - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[robot_description]) - - spawn = Node( - package='ros_ign_gazebo', executable='create', - arguments=[ - '-name', 'canadarm', - '-topic', robot_description, - ], - output='screen' - ) + # ) + run_move_arm = Node(package="canadarm_demo", executable="move_arm", output="screen") - # Control - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + return LaunchDescription( + [ + # run_node, + run_move_arm, + ] ) - - load_canadarm_joint_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'canadarm_joint_trajectory_controller'], - output='screen' - ) - - - - return LaunchDescription([ - start_world, - robot_state_publisher, - spawn, - #run_node, - run_move_arm, - - RegisterEventHandler( - OnProcessExit( - target_action=spawn, - on_exit=[load_joint_state_broadcaster], - ) - ), - RegisterEventHandler( - OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_canadarm_joint_controller], - ) - ), - ]) diff --git a/canadarm2/canadarm_demo/nodes/hello_moveit.cpp b/canadarm2/canadarm_demo/nodes/hello_moveit.cpp deleted file mode 100644 index 9e3f190..0000000 --- a/canadarm2/canadarm_demo/nodes/hello_moveit.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#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/canadarm2/canadarm_demo/nodes/move_arm b/canadarm2/canadarm_demo/nodes/move_arm index edc1f83..975d0ba 100755 --- a/canadarm2/canadarm_demo/nodes/move_arm +++ b/canadarm2/canadarm_demo/nodes/move_arm @@ -1,27 +1,45 @@ #!/usr/bin/env python3 +"""Move the arm to a specific position.""" -import rclpy -from rclpy.node import Node -from builtin_interfaces.msg import Duration +import rclpy # type: ignore +from rclpy.node import Node # type: ignore +from builtin_interfaces.msg import Duration # type: ignore + +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # type: ignore +from std_srvs.srv import Empty # type: ignore -from std_msgs.msg import String, Float64 -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from std_srvs.srv import Empty class MoveArm(Node): + """Move the arm to a specific position.""" 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) - + 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): # pylint: disable=unused-argument + """Open the arm.""" + traj = JointTrajectory() + traj.joint_names = [ + "Base_Joint", + "Shoulder_Roll", + "Shoulder_Yaw", + "Elbow_Pitch", + "Wrist_Pitch", + "Wrist_Yaw", + "Wrist_Roll", + ] - 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) @@ -29,13 +47,21 @@ class MoveArm(Node): traj.points.append(point1) self.arm_publisher_.publish(traj) - return response - def close_arm_callback(self, request, response): + def close_arm_callback(self, request, response): # pylint: disable=unused-argument + """Close the arm.""" traj = JointTrajectory() - traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"] - + 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) @@ -45,10 +71,19 @@ class MoveArm(Node): return response - def random_arm_callback(self, request, response): + def random_arm_callback(self, request, response): # pylint: disable=unused-argument + """Move the arm to a random position.""" traj = JointTrajectory() - traj.joint_names = ["Base_Joint", "Shoulder_Roll", "Shoulder_Yaw", "Elbow_Pitch", "Wrist_Pitch", "Wrist_Yaw", "Wrist_Roll"] - + 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) @@ -60,6 +95,7 @@ class MoveArm(Node): def main(args=None): + """Main function.""" rclpy.init(args=args) arm_node = MoveArm() @@ -72,5 +108,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/canadarm2/canadarm_demo/nodes/move_joint_server b/canadarm2/canadarm_demo/nodes/move_joint_server index d67eac9..63a2225 100755 --- a/canadarm2/canadarm_demo/nodes/move_joint_server +++ b/canadarm2/canadarm_demo/nodes/move_joint_server @@ -1,27 +1,26 @@ #!/usr/bin/env python3 +"""Create an action server to move the arm to a specific position.""" -import rclpy -from rclpy.action import ActionServer -from rclpy.node import Node -from builtin_interfaces.msg import Duration +import rclpy # type: ignore +from rclpy.action import ActionServer # type: ignore +from rclpy.node import Node # type: ignore -from std_msgs.msg import String, Float64, Float64MultiArray -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from std_srvs.srv import Empty +from std_msgs.msg import Float64MultiArray # type: ignore + +from canadarm_demo.action import MoveJoint -from canadarm.action import MoveJoint class MoveJointActionServer(Node): + """Action server to move the arm to a specific position.""" def __init__(self): - super().__init__('move_joint_node') - self.arm_publisher_ = self.create_publisher(Float64MultiArray, 'canadarm_joint_controller/commands',10) + super().__init__("move_joint_node") + self.arm_publisher_ = self.create_publisher( + Float64MultiArray, "canadarm_joint_controller/commands", 10 + ) self._action_server = ActionServer( - self, - MoveJoint, - 'movejoint', - self.movejoint_callback + self, MoveJoint, "movejoint", self.movejoint_callback ) timer_period = 0.01 # seconds @@ -30,21 +29,26 @@ class MoveJointActionServer(Node): self.goal_joint_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] def movejoint_callback(self, goal_handle): - self.get_logger().info('Executing joint goal...') + """Move the arm to a specific position.""" + self.get_logger().info("Executing joint goal...") self.goal_joint_pos = [float(i) for i in goal_handle.request.target] goal_handle.succeed() result = MoveJoint.Result() return result def move_joint(self): + """Move the arm to the goal position.""" target_val = Float64MultiArray() target_val.data = self.goal_joint_pos self.arm_publisher_.publish(target_val) def timer_callback(self): + """Timer callback to move the arm to the goal position.""" self.move_joint() + def main(args=None): + """Main function.""" rclpy.init(args=args) move_joint_node = MoveJointActionServer() @@ -55,5 +59,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/canadarm2/canadarm_demo/package.xml b/canadarm2/canadarm_demo/package.xml index 4535a26..8de1583 100644 --- a/canadarm2/canadarm_demo/package.xml +++ b/canadarm2/canadarm_demo/package.xml @@ -16,28 +16,16 @@ action_msgs ament_index_python - control_msgs - effort_controllers geometry_msgs - hardware_interface - ign_ros2_control - joint_state_broadcaster - joint_trajectory_controller launch launch_ros - robot_state_publisher - ros_ign_gazebo - ros2controlcli std_msgs - xacro rosidl_interface_packages - - ament_lint_auto - ament_lint_common - + ament_lint_auto + ament_lint_common ament_cmake diff --git a/canadarm2/canadarm_gazebo/package.xml b/canadarm2/canadarm_gazebo/package.xml index 04103e7..0a17ab1 100644 --- a/canadarm2/canadarm_gazebo/package.xml +++ b/canadarm2/canadarm_gazebo/package.xml @@ -2,18 +2,22 @@ canadarm_gazebo - 0.0.0 - TODO: Package description + 0.1.0 + Simulation description for CanadArm2 franklinselva - TODO: License declaration + GPL ament_cmake canadarm_description ros_gz ign_ros2_control + joint_state_broadcaster + robot_state_publisher + control_msgs ros2_control ros2_controllers + xacro ament_lint_auto ament_lint_common diff --git a/curiosity_rover/curiosity_rover_demo/package.xml b/curiosity_rover/curiosity_rover_demo/package.xml index ff78946..a0f0d92 100644 --- a/curiosity_rover/curiosity_rover_demo/package.xml +++ b/curiosity_rover/curiosity_rover_demo/package.xml @@ -19,18 +19,15 @@ effort_controllers geometry_msgs hardware_interface - ign_ros2_control imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller launch launch_ros robot_state_publisher - ros_ign_gazebo ros2controlcli std_msgs velocity_controllers - xacro ament_lint_auto ament_lint_common