Controlling the robot arm through /scaled_joint_trajectory_controller/joint_trajectory topic

[francescosemeraro]

Good afternoon!

In ROS 2 Humble, I would like to send joint configurations for the robot to assume through the /scaled_joint_trajectory_controller/joint_trajectory topic through a script in C++. I built a publisher that sends the exact positions of the test_scaled_joint_trajectory_controller.launch.py launch file. When I try to do that, the robot won't move. The message is received correctly by the topic, but for some reason the information is not processed. The launch file works okay. What am I missing? Is it possible that the launch file sets some options that cannot e seen in the script of publisher_joint-trajectory_controller.py?

`

#include "rclcpp/rclcpp.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "std_msgs/msg/float64_multi_array.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" #include "builtin_interfaces/msg/duration.hpp"

int main(int argc, char * argv[]) { // Initialize the ROS 2 node rclcpp::init(argc, argv); auto node = std::make_sharedrclcpp::Node("publisher_joint_trajectory"); auto pub = node->create_publisher<trajectory_msgs::msg::JointTrajectory>( "/scaled_joint_trajectory_controller/joint_trajectory", 1);

// Construct a JointTrajectory message containing the joint configuration
auto msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
msg->joint_names = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                    "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"};
auto point = std::make_shared<trajectory_msgs::msg::JointTrajectoryPoint>();
point->positions = {double(0.785), double(-1.57), double(1.23), double(0.785), double(0.785), double(0.0)}; // Set the desired joint configuration here
auto time_from_start = std::make_shared<builtin_interfaces::msg::Duration>();
time_from_start->sec = int(4);
point->time_from_start = *time_from_start; // Set the duration for this configuration
msg->points.push_back(*point);

// Publish the joint configuration
pub->publish(*msg);

// Spin the node until it is shutdown
rclcpp::spin(node);

// Shutdown the node
rclcpp::shutdown();

return 0;

} `

[santoshbalaji]

There are few things you can check.

1. First for JointTrajectoryPoint for all fields (velocities, accelerations) give same size of data given for positions. Just array with value 0 for all joints will do.
2. Also need to specify frame_id
3. Try to listen to joint_states and copy paste the same value with small values changes in positions to validate first.
4. Worst case still have issues then you can try actions instead of using topics.

Reference of working publisher message

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: world
joint_names:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
points:
- positions:
  - -1.4807089010821741
  - -1.7476030788817347
  - -1.458951473236084
  - -1.4788886916688462
  - 1.6500639915466309
  - -1.3739150206195276
  velocities:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  accelerations:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  effort: []
  time_from_start:
    sec: 0
    nanosec: 0

Happy coding !!!

[fmauch]

Just to add: Setting time_from_start might also be a good thing to do.

[fmauch]

I assume this can be closed. If there is still anything missing please comment and / or reopen.