dds::core::InvalidArgumentError when trying to send sample

[tmayoff]

I have a native dds application that communicates with ROS2, for the most part it works perfectly fine. However I just added a publisher for some joint trajectories, which now fails with the InvalidArgumentError.

The C++ snippet:

  trajectory_msgs::msg::dds_::JointTrajectory_ msg{};
  SetHeader(msg.header(), "base_link");
  std::vector<std::string> joint_names;
  joint_names.reserve(joint_targets.size());
  std::vector<double> positions;
  positions.reserve(joint_targets.size());
  for (const auto& [name, position] : joint_targets) {
    joint_names.push_back(name);
    positions.push_back(static_cast<double>(position));
  }

  msg.joint_names(joint_names);
  msg.points({trajectory_msgs::msg::dds_::JointTrajectoryPoint_{positions, {}, {}, {}, builtin_interfaces::msg::dds_::Duration_{0, 0}}});

  // Small abstraction calls write on the DataWriter
  const auto res = right_joint_publisher_->Write(msg);

If I don't put the positions into the trajectorymsgs::msg::dds::JointTrajectoryPoint_ struct it sends fine, but as it is above it throws the error.

#ifndef __trajectory_msgs__msg__JointTrajectory__idl
#define __trajectory_msgs__msg__JointTrajectory__idl

#include "std_msgs/msg/Header.idl"
#include "trajectory_msgs/msg/JointTrajectoryPoint.idl"

module trajectory_msgs {
  module msg {
    module dds_ {
      @final @topic struct JointTrajectory_ {
        std_msgs::msg::dds_::Header_ header;
        sequence<string> joint_names;
        sequence<trajectory_msgs::msg::dds_::JointTrajectoryPoint_> points;
      };
    };
  };
};

#endif // __trajectory_msgs__msg__JointTrajectory__idl

#ifndef __trajectory_msgs__msg__JointTrajectoryPoint__idl
#define __trajectory_msgs__msg__JointTrajectoryPoint__idl

#include "builtin_interfaces/msg/Duration.idl"

module trajectory_msgs {
  module msg {
    module dds_ {
      @final struct JointTrajectoryPoint_ {
        sequence<double> positions;
        sequence<double> velocities;
        sequence<double> accelerations;
        sequence<double> effort;
        builtin_interfaces::msg::dds_::Duration_ time_from_start;
      };
    };
  };
};

#endif // __trajectory_msgs__msg__JointTrajectoryPoint__idl

Error:

terminate called after throwing an instance of 'dds::core::InvalidArgumentError'
  what():  Error Bad Parameter - write failed.
===============================================================================
Context     : void org::eclipse::cyclonedds::pub::AnyDataWriterDelegate::write
Node        : UnknownNode

[eboasson]

I just tried it with master but hacking the "hello world" publisher and it seems to work fine. Which version are you using? And if it is (much) older, would it possible to update? (ROS 2 currently doesn't work with Cyclone master although there is a draft PR for that, but it does communicate fine with it over the network.)

//Publisher
#include "dds/dds.hpp"
#include "HelloWorldData.hpp"

using namespace org::eclipse::cyclonedds;

int main()
{
  dds::domain::DomainParticipant participant(0);
  dds::topic::Topic<trajectory_msgs::msg::dds_::JointTrajectory_> topic(participant, "topic");
  auto p_qos = participant.default_publisher_qos();
  dds::pub::Publisher publisher(participant, p_qos);
  auto qos = publisher.default_datawriter_qos();
  dds::pub::DataWriter right_joint_publisher_(publisher, topic, qos);

  trajectory_msgs::msg::dds_::JointTrajectory_ msg{};
  msg.header().stamp(builtin_interfaces::msg::dds_::Time_{3, 4});
  msg.header().frame_id("base_link");
  std::vector<std::string> joint_names;
  const int N = 3;
  joint_names.reserve(N);
  std::vector<double> positions;
  positions.reserve(N);

  joint_names.push_back("a");
  positions.push_back(1.0);
  joint_names.push_back("b");
  positions.push_back(2.0);
  joint_names.push_back("c");
  positions.push_back(3.0);

  msg.joint_names(joint_names);
  msg.points({trajectory_msgs::msg::dds_::JointTrajectoryPoint_{positions, {}, {}, {}, builtin_interfaces::msg::dds_::Duration_{0, 0}}});

  sleep(3);

  // Small abstraction calls write on the DataWriter
  right_joint_publisher_ << msg;

  sleep(1);
}

and

module builtin_interfaces {
  module msg {
    module dds_ {
      @final struct Time_ {
        int32 sec;
        int32 nanosec;
      };
      @final struct Duration_ {
        int32 secs;
        uint32 nanosec;
      };
    };
  };
};

module trajectory_msgs {
  module msg {
    module dds_ {
      @final struct JointTrajectoryPoint_ {
        sequence<double> positions;
        sequence<double> velocities;
        sequence<double> accelerations;
        sequence<double> effort;
        builtin_interfaces::msg::dds_::Duration_ time_from_start;
      };
    };
  };
};

module std_msgs {
  module msg {
    module dds_ {
      @final struct Header_ {
        builtin_interfaces::msg::dds_::Time_ stamp;
        string frame_id;
      };
    };
  };
};

module trajectory_msgs {
  module msg {
    module dds_ {
      @final @topic struct JointTrajectory_ {
        std_msgs::msg::dds_::Header_ header;
        sequence<string> joint_names;
        sequence<trajectory_msgs::msg::dds_::JointTrajectoryPoint_> points;
      };
    };
  };
};

[tmayoff]

We're using 0.10.5, (it's packaged manually by us into a debian package, so maybe something went wrong with building it? Although we build with all default settings). I'll try your test and see if it works

[tmayoff]

So I'm not sure what happened, I haven't changed to code but it's working now. Thanks for putting in the effort to help me out