-
Notifications
You must be signed in to change notification settings - Fork 317
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' into improve/memory_allocation/buffer_variables
- Loading branch information
Showing
98 changed files
with
4,530 additions
and
460 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
136 changes: 136 additions & 0 deletions
136
controller_interface/include/semantic_components/gps_sensor.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,136 @@ | ||
| // Copyright 2025 ros2_control development team | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ | ||
| #define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ | ||
|
|
||
| #include <array> | ||
| #include <string> | ||
|
|
||
| #include "semantic_components/semantic_component_interface.hpp" | ||
| #include "sensor_msgs/msg/nav_sat_fix.hpp" | ||
|
|
||
| namespace semantic_components | ||
| { | ||
|
|
||
| enum class GPSSensorOption | ||
| { | ||
| WithCovariance, | ||
| WithoutCovariance | ||
| }; | ||
|
|
||
| template <GPSSensorOption sensor_option> | ||
| class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix> | ||
| { | ||
| public: | ||
| static_assert( | ||
| sensor_option == GPSSensorOption::WithCovariance || | ||
| sensor_option == GPSSensorOption::WithoutCovariance, | ||
| "Invalid GPSSensorOption"); | ||
| explicit GPSSensor(const std::string & name) | ||
| : SemanticComponentInterface( | ||
| name, {{name + "/" + "status"}, | ||
| {name + "/" + "service"}, | ||
| {name + "/" + "latitude"}, | ||
| {name + "/" + "longitude"}, | ||
| {name + "/" + "altitude"}}) | ||
| { | ||
| if constexpr (sensor_option == GPSSensorOption::WithCovariance) | ||
| { | ||
| interface_names_.emplace_back(name + "/" + "latitude_covariance"); | ||
| interface_names_.emplace_back(name + "/" + "longitude_covariance"); | ||
| interface_names_.emplace_back(name + "/" + "altitude_covariance"); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Return GPS's status e.g. fix/no_fix | ||
| * | ||
| * \return Status | ||
| */ | ||
| int8_t get_status() const { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); } | ||
|
|
||
| /** | ||
| * Return service used by GPS e.g. fix/no_fix | ||
| * | ||
| * \return Service | ||
| */ | ||
| uint16_t get_service() const | ||
| { | ||
| return static_cast<uint16_t>(state_interfaces_[1].get().get_value()); | ||
| } | ||
|
|
||
| /** | ||
| * Return latitude reported by a GPS | ||
| * | ||
| * \return Latitude. | ||
| */ | ||
| double get_latitude() const { return state_interfaces_[2].get().get_value(); } | ||
|
|
||
| /** | ||
| * Return longitude reported by a GPS | ||
| * | ||
| * \return Longitude. | ||
| */ | ||
| double get_longitude() const { return state_interfaces_[3].get().get_value(); } | ||
|
|
||
| /** | ||
| * Return altitude reported by a GPS | ||
| * | ||
| * \return Altitude. | ||
| */ | ||
| double get_altitude() const { return state_interfaces_[4].get().get_value(); } | ||
|
|
||
| /** | ||
| * Return covariance reported by a GPS. | ||
| * | ||
| * \return Covariance array. | ||
| */ | ||
| template < | ||
| typename U = void, | ||
| typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>> | ||
| const std::array<double, 9> & get_covariance() | ||
| { | ||
| covariance_[0] = state_interfaces_[5].get().get_value(); | ||
| covariance_[4] = state_interfaces_[6].get().get_value(); | ||
| covariance_[8] = state_interfaces_[7].get().get_value(); | ||
| return covariance_; | ||
| } | ||
|
|
||
| /** | ||
| * Fills a NavSatFix message from the current values. | ||
| */ | ||
| bool get_values_as_message(sensor_msgs::msg::NavSatFix & message) | ||
| { | ||
| message.status.status = get_status(); | ||
| message.status.service = get_service(); | ||
| message.latitude = get_latitude(); | ||
| message.longitude = get_longitude(); | ||
| message.altitude = get_altitude(); | ||
|
|
||
| if constexpr (sensor_option == GPSSensorOption::WithCovariance) | ||
| { | ||
| message.position_covariance = get_covariance(); | ||
| } | ||
|
|
||
| return true; | ||
| } | ||
|
|
||
| private: | ||
| std::array<double, 9> covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; | ||
| }; | ||
|
|
||
| } // namespace semantic_components | ||
|
|
||
| #endif // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ |
77 changes: 77 additions & 0 deletions
77
controller_interface/include/semantic_components/led_rgb_device.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,77 @@ | ||
| // Copyright (c) 2024, Sherpa Mobile Robotics | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ | ||
| #define SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ | ||
|
|
||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| #include "semantic_components/semantic_component_command_interface.hpp" | ||
| #include "std_msgs/msg/color_rgba.hpp" | ||
|
|
||
| namespace semantic_components | ||
| { | ||
| class LedRgbDevice : public SemanticComponentCommandInterface<std_msgs::msg::ColorRGBA> | ||
| { | ||
| public: | ||
| /** | ||
| * Constructor for a LED RGB device with interface names set based on device name. | ||
| * The constructor sets the command interface names to "<name>/interface_r", | ||
| * "<name>/interface_g", "<name>/interface_b". | ||
| * | ||
| * \param[in] name name of the LED device, used as a prefix for the command interface names | ||
| * \param[in] interface_r name of the command interface for the red channel | ||
| * \param[in] interface_g name of the command interface for the green channel | ||
| * \param[in] interface_b name of the command interface for the blue channel | ||
| */ | ||
| explicit LedRgbDevice( | ||
| const std::string & name, const std::string & interface_r, const std::string & interface_g, | ||
| const std::string & interface_b) | ||
| : SemanticComponentCommandInterface( | ||
| name, {{name + "/" + interface_r}, {name + "/" + interface_g}, {name + "/" + interface_b}}) | ||
| { | ||
| } | ||
|
|
||
| /// Set LED states from ColorRGBA message | ||
|
|
||
| /** | ||
| * Set the values of the LED RGB device from a ColorRGBA message. | ||
| * | ||
| * \details Sets the values of the red, green, and blue channels from the message. | ||
| * If any of the values are out of the range [0, 1], the function fails and returns false. | ||
| * | ||
| * \param[in] message ColorRGBA message | ||
| * | ||
| * \return true if all values were set, false otherwise | ||
| */ | ||
| bool set_values_from_message(const std_msgs::msg::ColorRGBA & message) override | ||
| { | ||
| if ( | ||
| message.r < 0 || message.r > 1 || message.g < 0 || message.g > 1 || message.b < 0 || | ||
| message.b > 1) | ||
| { | ||
| return false; | ||
| } | ||
| bool all_set = true; | ||
| all_set &= command_interfaces_[0].get().set_value(message.r); | ||
| all_set &= command_interfaces_[1].get().set_value(message.g); | ||
| all_set &= command_interfaces_[2].get().set_value(message.b); | ||
| return all_set; | ||
| } | ||
| }; | ||
|
|
||
| } // namespace semantic_components | ||
|
|
||
| #endif // SEMANTIC_COMPONENTS__LED_RGB_DEVICE_HPP_ |
Oops, something went wrong.