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
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
Comments