diff --git a/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h b/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h index 9850e589..2e925a0f 100644 --- a/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h +++ b/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h @@ -40,7 +40,12 @@ namespace ROS2ScriptIntegration const AZStd::string& topicName, const AZStd::string& frame, const AZ::Transform& transform) = 0; virtual void PublishAckermannDriveMsg( const AZStd::string& topicName, float steeringAngle, float steeringVelocity, float speed, float acceleration, float jerk) = 0; - + virtual void PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) = 0; static void Reflect(AZ::ReflectContext* context); }; diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp index 2c92c893..4422a46f 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -211,6 +212,26 @@ namespace ROS2ScriptIntegration PublishMessage(topicName, message); } + void PublisherSystemComponent::PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) + { + sensor_msgs::msg::JointState message; + message.header.stamp = ROS2::ROS2ClockInterface::Get()->GetROSTimestamp(); + message.name.resize(names.size()); + for (size_t i = 0; i < names.size(); ++i) + { + message.name[i] = std::string(names[i].c_str()); + } + message.position = std::vector(positions.begin(), positions.end()); + message.velocity = std::vector(velocities.begin(), velocities.end()); + message.effort = std::vector(efforts.begin(), efforts.end()); + PublishMessage(topicName, message); + } + template std::shared_ptr> PublisherSystemComponent::GetOrCreatePublisher(const AZStd::string& topicName) { diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h index 4fc69f9b..ff6748c7 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h @@ -66,6 +66,13 @@ namespace ROS2ScriptIntegration const AZStd::string& topicName, float steeringAngle, float steeringVelocity, float speed, float acceleration, float jerk) override; + void PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) override; + // AZ::Component overrides ... void Init() override; diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp index 4889454b..5c53e233 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp @@ -53,7 +53,8 @@ namespace ROS2ScriptIntegration { "SteeringVelocity", "" }, { "Speed", "" }, { "Acceleration", "" }, - { "Jerk", "" } } }); + { "Jerk", "" } } }) + ->Event("PublishJointStateMsg", &PublisherRequestBus::Events::PublishJointStateMsg); } } } // namespace ROS2ScriptIntegration diff --git a/Gems/ROS2ScriptIntegration/README.md b/Gems/ROS2ScriptIntegration/README.md index 4438e7f9..62bcd686 100644 --- a/Gems/ROS2ScriptIntegration/README.md +++ b/Gems/ROS2ScriptIntegration/README.md @@ -9,6 +9,7 @@ Currently, the supported message types are limited to the following: - a subset of geometry messages - a subset of standard messages - joystick message +- joint state message ## Hello world publisher @@ -80,3 +81,47 @@ function myscript:OnStdMsgString(message) Debug.Log("I heard : " .. message) end ``` + +### Using messages with arrays (e.g. sensor_msgs/JointState) + +- you need to declare `vector_*` types to create C++ array equivalents +- you need to use `push_back` method to add elements to the array +- those arrays can be passed to the publisher method +Example: + +```lua +local LogJoints = +{ + Properties = {} +} + +function LogJoints:OnActivate() + self.tickBusHandler = TickBus.Connect(self) +end + +function LogJoints:OnDeactivate() + self.tickBusHandler:Disconnect() +end + +function LogJoints:OnTick(deltaTime, timePoint) + names = vector_basic_string_char_char_traits_char() + names:push_back("egoback_left_wheel_joint") + names:push_back("egoback_right_wheel_joint") + + joints = vector_float() + joints:push_back(0) + joints:push_back(0) + + velocities = vector_float() + velocities:push_back(0) + velocities:push_back(0) + + efforts = vector_float() + efforts:push_back(0) + efforts:push_back(0) + + PublisherRequestBus.Broadcast.PublishJointStateMsg("joint_states",names,joints,velocities,efforts) +end + +return LogJoints + ``` \ No newline at end of file