Update cart demos to use joint_trajectory_controller

gz_ros2_control_demos/CMakeLists.txt

@@ -37,16 +37,20 @@ ament_target_dependencies(example_position
   control_msgs
 )
 
-add_executable(example_velocity examples/example_velocity.cpp)
+# use the same example_position.cpp for example_velocity
+add_executable(example_velocity examples/example_position.cpp)
 ament_target_dependencies(example_velocity
   rclcpp
-  std_msgs
+  rclcpp_action
+  control_msgs
 )
 
-add_executable(example_effort examples/example_effort.cpp)
+# use the same example_position.cpp for example_effort
+add_executable(example_effort examples/example_position.cpp)
 ament_target_dependencies(example_effort
   rclcpp
-  std_msgs
+  rclcpp_action
+  control_msgs
 )
 
 add_executable(example_diff_drive examples/example_diff_drive.cpp)

gz_ros2_control_demos/config/cart_controller_effort.yaml

@@ -5,8 +5,20 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-effort_controller:
+joint_trajectory_controller:
   ros__parameters:
-    type: effort_controllers/JointGroupEffortController
+    type: joint_trajectory_controller/JointTrajectoryController
     joints:
       - slider_to_cart
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    gains:
+      slider_to_cart:
+        p: 100.0
+        i: 0.0
+        d: 10.0
+        i_clamp: 0.0
+        antiwindup: false

gz_ros2_control_demos/config/cart_controller_velocity.yaml

@@ -5,11 +5,23 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-velocity_controller:
+joint_trajectory_controller:
   ros__parameters:
-    type: velocity_controllers/JointGroupVelocityController
+    type: joint_trajectory_controller/JointTrajectoryController
     joints:
       - slider_to_cart
+    command_interfaces:
+      - velocity
+    state_interfaces:
+      - position
+      - velocity
+    gains:
+      slider_to_cart:
+        p: 100.0
+        i: 0.0
+        d: 0.0
+        i_clamp: 0.0
+        antiwindup: false
 
 imu_sensor_broadcaster:
   ros__parameters:

gz_ros2_control_demos/examples/example_position.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -26,6 +27,9 @@ bool common_goal_accepted = false;
 rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
 int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
 
+std::vector<double> desired_goals = {0, -1, 1, 0.0};
+unsigned int ct_goals_reached = 0;
+
 void common_goal_response(
   rclcpp_action::ClientGoalHandle
   <control_msgs::action::FollowJointTrajectory>::SharedPtr future)
@@ -47,21 +51,21 @@ void common_result_response(
   const rclcpp_action::ClientGoalHandle
   <control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
 {
-  printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
+  printf("common_result_response time: %f\n", rclcpp::Clock(RCL_ROS_TIME).now().seconds());
   common_resultcode = result.code;
   common_action_result_code = result.result->error_code;
   switch (result.code) {
     case rclcpp_action::ResultCode::SUCCEEDED:
-      printf("SUCCEEDED result code\n");
+      printf("Action goal succeeded\n");
       break;
     case rclcpp_action::ResultCode::ABORTED:
-      printf("Goal was aborted\n");
+      printf("Action goal was aborted\n");
       return;
     case rclcpp_action::ResultCode::CANCELED:
-      printf("Goal was canceled\n");
+      printf("Action goal was canceled\n");
       return;
     default:
-      printf("Unknown result code\n");
+      printf("Action goal: Unknown result code\n");
       return;
   }
 }
@@ -70,25 +74,32 @@ void common_feedback(
   rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,
   const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback)
 {
-  std::cout << "feedback->desired.positions :";
-  for (auto & x : feedback->desired.positions) {
-    std::cout << x << "\t";
-  }
+  std::cout << "feedback->desired: positions: " << feedback->desired.positions.at(0);
+  std::cout << ", velocities: " << feedback->desired.velocities.at(0) << std::endl;
+
+  std::cout << "feedback->actual: positions: " << feedback->actual.positions.at(0);
   std::cout << std::endl;
-  std::cout << "feedback->desired.velocities :";
-  for (auto & x : feedback->desired.velocities) {
-    std::cout << x << "\t";
+
+  if (ct_goals_reached < desired_goals.size()) {
+    if (fabs(feedback->actual.positions[0] - desired_goals.at(ct_goals_reached)) < 0.1) {
+      std::cout << "Goal # " << ct_goals_reached << ": " << desired_goals.at(ct_goals_reached) <<
+        " reached" << std::endl;
+      ct_goals_reached++;
+      if (ct_goals_reached < desired_goals.size()) {
+        std::cout << "next goal # " << ct_goals_reached << ": " <<
+          desired_goals.at(ct_goals_reached) <<
+          std::endl;
+      }
+    }
   }
-  std::cout << std::endl;
 }
 
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-
   node = std::make_shared<rclcpp::Node>("trajectory_test_node");
 
-  std::cout << "node created" << std::endl;
+  RCLCPP_DEBUG(node->get_logger(), "node created");
 
   rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
   action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
@@ -98,36 +109,43 @@ int main(int argc, char * argv[])
     node->get_node_waitables_interface(),
     "/joint_trajectory_controller/follow_joint_trajectory");
 
-  bool response =
-    action_client->wait_for_action_server(std::chrono::seconds(1));
-  if (!response) {
-    throw std::runtime_error("could not get action server");
+  while (true) {
+    bool response =
+      action_client->wait_for_action_server(std::chrono::seconds(1));
+    if (!response) {
+      using namespace std::chrono_literals;
+      std::this_thread::sleep_for(2000ms);
+      RCLCPP_WARN(node->get_logger(), "Trying to connect to the server again");
+      continue;
+    } else {
+      break;
+    }
   }
-  std::cout << "Created action server" << std::endl;
+
+  RCLCPP_DEBUG(node->get_logger(), "Created action server");
 
   std::vector<std::string> joint_names = {"slider_to_cart"};
 
   std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
   trajectory_msgs::msg::JointTrajectoryPoint point;
-  point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
+  point.time_from_start = rclcpp::Duration::from_seconds(1.0);
   point.positions.resize(joint_names.size());
-
-  point.positions[0] = 5.0;
+  point.positions[0] = desired_goals[0];
 
   trajectory_msgs::msg::JointTrajectoryPoint point2;
-  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.time_from_start = rclcpp::Duration::from_seconds(2.0);
   point2.positions.resize(joint_names.size());
-  point2.positions[0] = -1.0;
+  point2.positions[0] = desired_goals[1];
 
   trajectory_msgs::msg::JointTrajectoryPoint point3;
-  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.time_from_start = rclcpp::Duration::from_seconds(3.0);
   point3.positions.resize(joint_names.size());
-  point3.positions[0] = 1.0;
+  point3.positions[0] = desired_goals[2];
 
   trajectory_msgs::msg::JointTrajectoryPoint point4;
-  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.time_from_start = rclcpp::Duration::from_seconds(4.0);
   point4.positions.resize(joint_names.size());
-  point4.positions[0] = 0.0;
+  point4.positions[0] = desired_goals[3];
 
   points.push_back(point);
   points.push_back(point2);
@@ -144,6 +162,7 @@ int main(int argc, char * argv[])
   goal_msg.trajectory.joint_names = joint_names;
   goal_msg.trajectory.points = points;
 
+  RCLCPP_DEBUG(node->get_logger(), "async_send_goal");
   auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
 
   if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
@@ -173,11 +192,20 @@ int main(int argc, char * argv[])
     rclcpp::FutureReturnCode::SUCCESS)
   {
     RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
+    action_client.reset();
+    node.reset();
     return 1;
   }
 
   action_client.reset();
   node.reset();
+
+  if (desired_goals.size() != ct_goals_reached) {
+    RCLCPP_ERROR(node->get_logger(), "Not all the goals were reached");
+    rclcpp::shutdown();
+    return -1;
+  }
+
   rclcpp::shutdown();
 
   return 0;

gz_ros2_control_demos/launch/ackermann_drive_example.launch.py

@@ -96,7 +96,7 @@ def robot_state_publisher(context):
                 [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
                                        'launch',
                                        'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+            launch_arguments=[('gz_args', [' -r -v 1 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,