Robotics StackExchange | Archived questions

Why is trajectory_msgs__msg__JointTrajectory not valid for Publisher

When I try to define a sharedptr for a publisher with message type of trajectorymsgs_msgJointTrajectory I get an error message :

/home/guenther/myrobot_ws/src/myrobot_controller_hardware/src/myrobot_system_position_only.cpp:46:61:   required from here
/opt/ros/humble/include/rclcpp/rclcpp/publisher.hpp:81:47: error: static assertion failed: given message type is not compatible with ROS and cannot be used with a Publisher
   81 |     rclcpp::is_ros_compatible_type<MessageT>::value,

when creating a shared pointer with:

rclcpp::Publisher<trajectory_msgs__msg__JointTrajectory>::SharedPtr publisher_; 

Is this caused by the message type or by the way the pointer is defined? A standard string message type gives no error.

Update:

    class HardwareCommandPub : public rclcpp::Node
    {
      public:
        HardwareCommandPub() : Node("ToArduino_command_publisher")
    {
      publisher_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("Joint_Value", 10);
    //  publisher_ = this->create_publisher<std_msgs::msg::Float32>("Joint_Value", 10);
    }

    void publishData(trajectory_msgs::msg::JointTrajectory message)
    //void publishData(std_msgs::msg::Float32 message)
    {
      publisher_->publish(message);
    }
        rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr publisher_;
    //    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr publisher_;
    };

namespace myrobotcontrollerhardware {

auto hwcmd = new trajectorymsgs::msg::JointTrajectory(); auto hwcmdpub = new HardwareCommandPub(); //fire up the publisher node

hardwareinterface::CallbackReturn myrobotSystemPositionOnlyHardware::oninit( const hardware_interface::HardwareInfo & info) {

  if (
    hardware_interface::SystemInterface::on_init(info) !=
    hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }


  // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
  hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
  hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
  hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
  // END: This part here is for exemplary purposes - Please do not copy to your production code
  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  hw_cmd_->joint_names[0] = "hip";
  hw_cmd_->joint_names[1] = "shoulder";
  hw_cmd_->joint_names[2] = "elbow";
  hw_cmd_->joint_names[3] = "wrist"; 
  hw_cmd_->points[0].positions[0] = 0.0;
  hw_cmd_->points[0].positions[1] = 0.0;
  hw_cmd_->points[0].positions[2] = 0.0;
  hw_cmd_->points[0].positions[3] = 0.0;

.....
  return hardware_interface::CallbackReturn::SUCCESS;
}

}

In another member function I call the publisher but there I get an error:

hardware_interface::return_type myrobotSystemPositionOnlyHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{

  // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
//  RCLCPP_INF    hardware_interface::return_type myrobotSystemPositionOnlyHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{

  // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
//  RCLCPP_INFO(rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Writing...");

  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    // Simulate sending commands to the hardware
    RCLCPP_INFO(
      rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
      hw_commands_[i], i);

    hw_cmd_->points[0].positions[i] = hw_commands_[i];
  }
  hw_cmd_pub_->publishData(hw_cmd_);
//  RCLCPP_INFO(
//    rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Joints successfully written!");
  // END: This part here is for exemplary purposes - Please do not copy to your production code

  return hardware_interface::return_type::OK;

}O(rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Writing...");

  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    // Simulate sending commands to the hardware
    RCLCPP_INFO(
      rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
      hw_commands_[i], i);

    hw_cmd_->points[0].positions[i] = hw_commands_[i];
  }
  hw_cmd_pub_->publishData(hw_cmd_);
//  RCLCPP_INFO(
//    rclcpp::get_logger("myrobotSystemPositionOnlyHardware"), "Joints successfully written!");
  // END: This part here is for exemplary purposes - Please do not copy to your production code

  return hardware_interface::return_type::OK;

}

Error message:

 /home/guenther/myrobot_ws/src/myrobot_controller_hardware/src/myrobot_system_position_only.cpp: In member function ‘virtual hardware_interface::return_type myrobot_controller_hardware::myrobotSystemPositionOnlyHardware::write(const rclcpp::Time&, const rclcpp::Duration&)’:
    /home/guenther/myrobot_ws/src/myrobot_controller_hardware/src/myrobot_system_position_only.cpp:271:28: error: cannot convert ‘trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >*’ to ‘trajectory_msgs::msg::JointTrajectory’ {aka ‘trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >’}
      271 |   hw_cmd_pub_->publishData(hw_cmd_);
          |                            ^~~~~~~
          |                            |
          |                            trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >*

Asked by GUENNI on 2022-11-07 08:51:32 UTC

Comments

Answers

This ros2 example uses:

// Declaration of the publisher_ attribute
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

so I'd expect you also need to use the c++ namespace syntax:

trajectory_msgs::msg::JointTrajectory

Asked by Mike Scheutzow on 2022-11-07 21:02:06 UTC

Comments

trajectory_msgs__msg__JointTrajectory is indeed the C type.

Asked by gvdhoorn on 2022-11-08 05:11:00 UTC

Thank you for your answer. It helped me to get rid of most errors. I update my question with the corrected version. There is one error left. I hope for another good hint.

I am confused by the two styles trajectory_msgs__msg__JointTrajectory and trajectory_msgs::msg::JointTrajectory.
Do I understand it right one is C notation the other is C++?

Asked by GUENNI on 2022-11-08 08:54:13 UTC