diff --git a/canadarm/CMakeLists.txt b/canadarm/CMakeLists.txt index f782dc43..80899a72 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 97a4f748..627913c6 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 1677d369..ce12266b 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 b6d17ce8..91a843f0 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/move_arm b/canadarm/nodes/move_arm new file mode 100755 index 00000000..edc1f833 --- /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 478860db..64417606 100644 --- a/canadarm/worlds/simple.world +++ b/canadarm/worlds/simple.world @@ -47,7 +47,7 @@ - /home/spaceros-user/demos_ws/install/simulation/share/simulation/models/canadarm/meshes/earth.dae + model://canadarm/meshes/earth.dae 3 3 3 @@ -77,7 +77,7 @@ - /home/spaceros-user/demos_ws/install/simulation/share/simulation/models/canadarm/meshes/iss.dae + model://canadarm/meshes/iss.dae 1 1 1