Resolve conflicts

Author: Ubuntu

ur_bringup/config/ur_controllers.yaml

@@ -24,35 +24,45 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+    passthrough_trajectory_controller:
+      type: ur_controllers/PassthroughTrajectoryController
+
+    tcp_pose_broadcaster:
+      type: pose_broadcaster/PoseBroadcaster
+
 
 speed_scaling_state_broadcaster:
   ros__parameters:
     state_publish_rate: 100.0
+    tf_prefix: "$(var tf_prefix)"
 
+io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
 
 force_torque_sensor_broadcaster:
   ros__parameters:
-    sensor_name: tcp_fts_sensor
+    sensor_name: $(var tf_prefix)tcp_fts_sensor
     state_interface_names:
       - force.x
       - force.y
       - force.z
       - torque.x
       - torque.y
       - torque.z
-    frame_id: tool0
+    frame_id: $(var tf_prefix)tool0
     topic_name: ft_data
 
 
 joint_trajectory_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     command_interfaces:
       - position
     state_interfaces:
@@ -64,23 +74,23 @@ joint_trajectory_controller:
     constraints:
       stopped_velocity_tolerance: 0.2
       goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
 
 
 scaled_joint_trajectory_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     command_interfaces:
       - position
     state_interfaces:
@@ -92,30 +102,46 @@ scaled_joint_trajectory_controller:
     constraints:
       stopped_velocity_tolerance: 0.2
       goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
+
+passthrough_trajectory_controller:
+  ros__parameters:
+    tf_prefix: "$(var tf_prefix)"
+    joints:
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
+    state_interfaces:
+      - position
+      - velocity
+    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor
 
 forward_velocity_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint
     interface_name: velocity
 
 forward_position_controller:
   ros__parameters:
     joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
+      - $(var tf_prefix)shoulder_pan_joint
+      - $(var tf_prefix)shoulder_lift_joint
+      - $(var tf_prefix)elbow_joint
+      - $(var tf_prefix)wrist_1_joint
+      - $(var tf_prefix)wrist_2_joint
+      - $(var tf_prefix)wrist_3_joint

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -327,7 +327,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
         scaling_factor_ = scaling_state_interface_->get().get_value();
       }
 
-      active_trajectory_elapsed_time_ += period * scaling_factor_;
+      active_trajectory_elapsed_time_ = active_trajectory_elapsed_time_ + (period * scaling_factor_);
 
       // RCLCPP_INFO(get_node()->get_logger(), "Elapsed trajectory time: %f. Scaling factor: %f, period: %f",
       // active_trajectory_elapsed_time_.seconds(), scaling_factor_, period.seconds());
@@ -356,7 +356,7 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb
 {
   RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory.");
   // Precondition: Running controller
-  if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
+  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
     RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running.");
     return rclcpp_action::GoalResponse::REJECT;
   }

ur_robot_driver/CMakeLists.txt

@@ -156,6 +156,7 @@ ament_python_install_package(${PROJECT_NAME})
 # Install Python executables
 install(PROGRAMS
   scripts/tool_communication.py
+  scripts/example_move.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/config/ur_controllers.yaml

@@ -24,16 +24,12 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
-<<<<<<< HEAD
-=======
     passthrough_trajectory_controller:
       type: ur_controllers/PassthroughTrajectoryController
+
     tcp_pose_broadcaster:
       type: pose_broadcaster/PoseBroadcaster
 
->>>>>>> ca5fb3e (Add trajectory passthrough controller (#944))
-    ur_configuration_controller:
-      type: ur_controllers/URConfigurationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -44,10 +40,6 @@ io_and_status_controller:
   ros__parameters:
     tf_prefix: "$(var tf_prefix)"
 
-ur_configuration_controller:
-  ros__parameters:
-    tf_prefix: "$(var tf_prefix)"
-
 force_torque_sensor_broadcaster:
   ros__parameters:
     sensor_name: $(var tf_prefix)tcp_fts_sensor

ur_robot_driver/launch/ur_control.launch.py

@@ -295,7 +295,6 @@ def launch_setup(context, *args, **kwargs):
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
-                    "ur_configuration_controller",
                 ]
             },
         ],
@@ -338,11 +337,7 @@ def controller_spawner(controllers, active=True):
         "io_and_status_controller",
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
-        "ur_configuration_controller",
     ]
-<<<<<<< HEAD
-    controllers_inactive = ["forward_position_controller"]
-=======
     controllers_inactive = [
         "scaled_joint_trajectory_controller",
         "joint_trajectory_controller",
@@ -353,39 +348,11 @@ def controller_spawner(controllers, active=True):
     if activate_joint_controller.perform(context) == "true":
         controllers_active.append(initial_joint_controller.perform(context))
         controllers_inactive.remove(initial_joint_controller.perform(context))
->>>>>>> ca5fb3e (Add trajectory passthrough controller (#944))
 
     controller_spawners = [controller_spawner(controllers_active)] + [
         controller_spawner(controllers_inactive, active=False)
     ]
 
-    # There may be other controllers of the joints, but this is the initially-started one
-    initial_joint_controller_spawner_started = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-        ],
-        condition=IfCondition(activate_joint_controller),
-    )
-    initial_joint_controller_spawner_stopped = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-            "--inactive",
-        ],
-        condition=UnlessCondition(activate_joint_controller),
-    )
-
     nodes_to_start = [
         control_node,
         ur_control_node,
@@ -395,8 +362,6 @@ def controller_spawner(controllers, active=True):
         urscript_interface,
         robot_state_publisher_node,
         rviz_node,
-        initial_joint_controller_spawner_stopped,
-        initial_joint_controller_spawner_started,
     ] + controller_spawners
 
     return nodes_to_start
@@ -525,16 +490,13 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "initial_joint_controller",
             default_value="scaled_joint_trajectory_controller",
-<<<<<<< HEAD
-=======
             choices=[
                 "scaled_joint_trajectory_controller",
                 "joint_trajectory_controller",
                 "forward_velocity_controller",
                 "forward_position_controller",
                 "passthrough_trajectory_controller",
             ],
->>>>>>> ca5fb3e (Add trajectory passthrough controller (#944))
             description="Initially loaded robot controller.",
         )
     )

ur_robot_driver/scripts/example_move.py

@@ -48,24 +48,24 @@
     "traj0": [
         {
             "positions": [0.043128, -1.28824, 1.37179, -1.82208, -1.63632, -0.18],
-            "velocities": [0, 0, 0, 0, 0, 0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             "time_from_start": Duration(sec=4, nanosec=0),
         },
         {
             "positions": [-0.195016, -1.70093, 0.902027, -0.944217, -1.52982, -0.195171],
-            "velocities": [0, 0, 0, 0, 0, 0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             "time_from_start": Duration(sec=8, nanosec=0),
         },
     ],
     "traj1": [
         {
             "positions": [-0.195016, -1.70094, 0.902027, -0.944217, -1.52982, -0.195171],
-            "velocities": [0, 0, 0, 0, 0, 0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             "time_from_start": Duration(sec=0, nanosec=0),
         },
         {
             "positions": [0.30493, -0.982258, 0.955637, -1.48215, -1.72737, 0.204445],
-            "velocities": [0, 0, 0, 0, 0, 0],
+            "velocities": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             "time_from_start": Duration(sec=8, nanosec=0),
         },
     ],

ur_robot_driver/src/hardware_interface.cpp

@@ -887,7 +887,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
 
   if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
                    [&](const std::string& other) { return other == start_modes_[0]; })) {
-    RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
+    RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Start modes of all joints have to be the same.");
     return hardware_interface::return_type::ERROR;
   }