-
Notifications
You must be signed in to change notification settings - Fork 193
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
74 changed files
with
2,419 additions
and
1,072 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,86 @@ | ||
| #pragma once | ||
| #include "depthai-shared/common/CameraFeatures.hpp" | ||
| #include "depthai/device/CalibrationHandler.hpp" | ||
| #include "geometry_msgs/Quaternion.h" | ||
| #include "geometry_msgs/TransformStamped.h" | ||
| #include "nlohmann/json.hpp" | ||
| #include "robot_state_publisher/robot_state_publisher.h" | ||
| #include "ros/node_handle.h" | ||
| #include "tf2_ros/static_transform_broadcaster.h" | ||
|
|
||
| namespace dai { | ||
| namespace ros { | ||
| class TFPublisher { | ||
| public: | ||
| explicit TFPublisher(::ros::NodeHandle node, | ||
| const dai::CalibrationHandler& calHandler, | ||
| const std::vector<dai::CameraFeatures>& camFeatures, | ||
| const std::string& camName, | ||
| const std::string& camModel, | ||
| const std::string& baseFrame, | ||
| const std::string& parentFrame, | ||
| const std::string& camPosX, | ||
| const std::string& camPosY, | ||
| const std::string& camPosZ, | ||
| const std::string& camRoll, | ||
| const std::string& camPitch, | ||
| const std::string& camYaw, | ||
| const std::string& imuFromDescr, | ||
| const std::string& customURDFLocation, | ||
| const std::string& customXacroArgs); | ||
| /* | ||
| * @brief Obtain URDF description by running Xacro with provided arguments. | ||
| */ | ||
| std::string getURDF(); | ||
| geometry_msgs::Quaternion quatFromRotM(nlohmann::json rotMatrix); | ||
| geometry_msgs::Vector3 transFromExtr(nlohmann::json translation); | ||
|
|
||
| private: | ||
| /* | ||
| * @brief Converts model name to one of the available model families | ||
| */ | ||
| void convertModelName(); | ||
| /* | ||
| * @brief Prepare arguments for xacro command. If custom URDF location is not provided, check if model name is available in depthai_descriptions package. | ||
| */ | ||
| std::string prepareXacroArgs(); | ||
| /* | ||
| * @brief Get URDF description and set it as a parameter for robot_state_publisher | ||
| */ | ||
| void publishDescription(::ros::NodeHandle node); | ||
| /* | ||
| * @brief Publish camera transforms ("standard" and optical) based on calibration data. | ||
| * Frame names are based on socket names and use following convention: [base_frame]_[socket_name]_camera_frame and | ||
| * [base_frame]_[socket_name]_camera_optical_frame | ||
| */ | ||
| void publishCamTransforms(nlohmann::json camData, ::ros::NodeHandle node); | ||
| /* | ||
| * @brief Publish IMU transform based on calibration data. | ||
| * Frame name is based on IMU name and uses following convention: [base_frame]_imu_frame. | ||
| * If IMU extrinsics are not set, warning is printed out and imu frame is published with zero translation and rotation. | ||
| */ | ||
| void publishImuTransform(nlohmann::json json, ::ros::NodeHandle node); | ||
| /* | ||
| * @brief Check if model STL file is available in depthai_descriptions package. | ||
| */ | ||
| bool modelNameAvailable(); | ||
| std::string getCamSocketName(int socketNum); | ||
| std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tfPub; | ||
| std::shared_ptr<robot_state_publisher::RobotStatePublisher> _rsp; | ||
| std::string _camName; | ||
| std::string _camModel; | ||
| std::string _baseFrame; | ||
| std::string _parentFrame; | ||
| std::string _camPosX; | ||
| std::string _camPosY; | ||
| std::string _camPosZ; | ||
| std::string _camRoll; | ||
| std::string _camPitch; | ||
| std::string _camYaw; | ||
| std::string _imuFromDescr; | ||
| std::string _customURDFLocation; | ||
| std::string _customXacroArgs; | ||
| std::vector<dai::CameraFeatures> _camFeatures; | ||
| }; | ||
| } // namespace ros | ||
| } // namespace dai |
56 changes: 56 additions & 0 deletions
56
depthai_bridge/include/depthai_bridge/TrackedFeaturesConverter.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,56 @@ | ||
| #pragma once | ||
|
|
||
| #include <depthai_ros_msgs/TrackedFeatures.h> | ||
|
|
||
| #include <deque> | ||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| #include "depthai/pipeline/datatype/TrackedFeatures.hpp" | ||
| #include "ros/time.h" | ||
|
|
||
| namespace dai { | ||
|
|
||
| namespace ros { | ||
|
|
||
| class TrackedFeaturesConverter { | ||
| public: | ||
| // DetectionConverter() = default; | ||
| TrackedFeaturesConverter(std::string frameName, bool getBaseDeviceTimestamp = false); | ||
| ~TrackedFeaturesConverter(); | ||
|
|
||
| /** | ||
| * @brief Handles cases in which the ROS time shifts forward or backward | ||
| * Should be called at regular intervals or on-change of ROS time, depending | ||
| * on monitoring. | ||
| * | ||
| */ | ||
| void updateRosBaseTime(); | ||
|
|
||
| /** | ||
| * @brief Commands the converter to automatically update the ROS base time on message conversion based on variable | ||
| * | ||
| * @param update: bool whether to automatically update the ROS base time on message conversion | ||
| */ | ||
| void setUpdateRosBaseTimeOnToRosMsg(bool update = true) { | ||
| _updateRosBaseTimeOnToRosMsg = update; | ||
| } | ||
|
|
||
| void toRosMsg(std::shared_ptr<dai::TrackedFeatures> inFeatures, std::deque<depthai_ros_msgs::TrackedFeatures>& featureMsgs); | ||
|
|
||
| private: | ||
| const std::string _frameName; | ||
| std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime; | ||
| ::ros::Time _rosBaseTime; | ||
| bool _getBaseDeviceTimestamp; | ||
| // For handling ROS time shifts and debugging | ||
| int64_t _totalNsChange{0}; | ||
| // Whether to update the ROS base time on each message conversion | ||
| bool _updateRosBaseTimeOnToRosMsg{false}; | ||
| }; | ||
|
|
||
| } // namespace ros | ||
|
|
||
| namespace rosBridge = ros; | ||
|
|
||
| } // namespace dai |
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
Oops, something went wrong.