From 5863bf85e61198e6b9f3da84aef72683b22e510e Mon Sep 17 00:00:00 2001 From: Ethan Date: Wed, 17 Dec 2025 15:04:41 +1100 Subject: [PATCH 01/13] Added velocities to odometry message --- .../frontend/RGBDInstanceFrontendModule.hpp | 7 +++ .../frontend/VisionImuOutputPacket.hpp | 11 +++- .../frontend/vision/FeatureTracker.hpp | 2 +- .../frontend/RGBDInstanceFrontendModule.cc | 7 +++ dynosam/src/frontend/VisionImuOutputPacket.cc | 28 +++++++++ dynosam/src/frontend/vision/FeatureTracker.cc | 53 +++++++++++++++- .../include/dynosam_common/DynamicObjects.hpp | 6 +- dynosam_common/src/DynamicObjects.cc | 39 +++++++++++- .../include/dynosam_cv/ImageContainer.hpp | 5 ++ dynosam_cv/src/ImageContainer.cc | 15 +++++ .../dynosam_ros/displays/DisplaysCommon.hpp | 3 +- .../dynamic_slam_displays/DSDCommonRos.hpp | 8 ++- dynosam_ros/src/RosUtils.cc | 18 ++++++ dynosam_ros/src/displays/DisplaysCommon.cc | 12 +++- .../dynamic_slam_displays/DSDCommonRos.cc | 61 +++++++++++++++---- .../dynamic_slam_displays/FrontendDSDRos.cc | 7 ++- dynosam_utils/src/run_experiments_ecmr.py | 17 +++--- 17 files changed, 259 insertions(+), 40 deletions(-) diff --git a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp index 1b17db39a..3545f3584 100644 --- a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp @@ -126,6 +126,13 @@ class RGBDInstanceFrontendModule : public FrontendModule { //! Last keyframe Frame::Ptr frame_lkf_; + + // Previous packet + VisionImuPacket::Ptr previous_vision_imu_packet_; + + }; + + } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp index 563951a06..286b44583 100644 --- a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp +++ b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp @@ -80,7 +80,7 @@ class VisionImuPacket { }; /** - * @brief Object track information epresenting visual measurements for a + * @brief Object track information representing visual measurements for a * single object as well as frame-to-frame (and possibly other) motion/pose * information. * @@ -99,6 +99,8 @@ class VisionImuPacket { ImuFrontend::PimPtr pim() const; Camera::ConstPtr camera() const; PointCloudLabelRGB::Ptr denseLabelledCloud() const; + gtsam::Vector6 getBodyVelocity() const; + bool isCameraKeyFrame() const; bool isObjectKeyFrame(ObjectId object_id) const; @@ -107,6 +109,7 @@ class VisionImuPacket { const CameraTracks& cameraTracks() const; const gtsam::Pose3& cameraPose() const; + /** * @brief Returns the relative camera motion T_k_1_k, representing the motion * of the camera from k-1 to k in the camera local frame (at k-1) @@ -149,6 +152,7 @@ class VisionImuPacket { VisionImuPacket& groundTruthPacket( const GroundTruthInputPacket::Optional& gt); VisionImuPacket& debugImagery(const DebugImagery::Optional& dbg); + VisionImuPacket& updateBodyVelocity(const VisionImuPacket& prev_state); bool operator==(const VisionImuPacket& other) const { return frame_id_ == other.frame_id_ && timestamp_ == other.timestamp_; @@ -196,6 +200,11 @@ class VisionImuPacket { static void fillLandmarkMeasurements( StatusLandmarkVector& landmarks, const CameraMeasurementStatusVector& camera_measurements); + + + private: + std::optional stored_body_velocity; + }; } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp index 7040d271f..7391d93da 100644 --- a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp +++ b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp @@ -146,7 +146,7 @@ class FeatureTracker : public FeatureTrackerBase { private: // TODO: for now we loose the actual object detection result if inference was // run! - bool objectDetection( + std::optional objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container); diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index 12a990edb..50fff5fac 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -231,6 +231,13 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( fillOutputPacketWithTracks(vision_imu_packet, *frame, T_k_1_k, object_motions, object_poses); + // Update the body velocity according to the previous vision IMU packet + if (previous_vision_imu_packet_) { + vision_imu_packet->updateBodyVelocity(*previous_vision_imu_packet_); + } + + previous_vision_imu_packet_ = vision_imu_packet; + if (R_curr_ref) { imu_frontend_.resetIntegration(); } diff --git a/dynosam/src/frontend/VisionImuOutputPacket.cc b/dynosam/src/frontend/VisionImuOutputPacket.cc index e4c24fe02..5d1e79eca 100644 --- a/dynosam/src/frontend/VisionImuOutputPacket.cc +++ b/dynosam/src/frontend/VisionImuOutputPacket.cc @@ -40,6 +40,8 @@ ImuFrontend::PimPtr VisionImuPacket::pim() const { return pim_; } Camera::ConstPtr VisionImuPacket::camera() const { return camera_; } +gtsam::Vector6 VisionImuPacket::getBodyVelocity() const { return *stored_body_velocity; } + PointCloudLabelRGB::Ptr VisionImuPacket::denseLabelledCloud() const { return dense_labelled_cloud_; } @@ -213,6 +215,32 @@ void VisionImuPacket::fillLandmarkMeasurements( } } +// Adding velocity information into the VisionImuPacket + +// * Function to update the previous body velocity * +VisionImuPacket& VisionImuPacket::updateBodyVelocity(const VisionImuPacket& prev_state) { + + // Getting the pose at k-1 + const gtsam::Pose3& w_L_k_1 = prev_state.cameraPose(); + + // Getting the motion from k-1 to k + const gtsam::Pose3& w_k_1_H_k = camera_tracks_.T_k_1_k; + + stored_body_velocity = calculateBodyMotion( + w_k_1_H_k, + w_L_k_1, + prev_state.timestamp(), + timestamp_ + ); + + return *this; + +} + +// * Function to get the body velocity * + + + } // namespace dyno namespace nlohmann { diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index 7169c3bfa..888d80a00 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -47,6 +47,32 @@ namespace dyno { + // HELPER FUNCTION ADDED BY ETHAN +// map for classes to number IDs (so we can expand later) +static const std::map class_name_to_id{ + {"Undefined", ClassSegmentation::Undefined}, // 0 + {"Road", ClassSegmentation::Road}, // 1 + {"Rider", ClassSegmentation::Rider}, // 2 +}; + +int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { + + auto it = object_classes.find(obj_id); // find the object in the map + + if (it == object_classes.end()) { + return ClassSegmentation::Undefined; + } + + auto class_it = class_name_to_id.find(it->second); + if (class_it != class_name_to_id.end()) { + return class_it->second; + } + + return ClassSegmentation::Undefined; + +} + + FeatureTracker::FeatureTracker(const FrontendParams& params, Camera::Ptr camera, ImageDisplayQueue* display_queue) : FeatureTrackerBase(params.tracker_params, camera, display_queue), @@ -102,6 +128,7 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // result after the function call) ObjectDetectionEngine is used if // params_.prefer_provided_object_detection is false vision_tools::ObjectBoundaryMaskResult boundary_mask_result; + objectDetection(boundary_mask_result, input_images); if (!initial_computation_ && params_.use_propogate_mask) { @@ -1151,7 +1178,7 @@ void FeatureTracker::requiresSampling( } } -bool FeatureTracker::objectDetection( +std::optional FeatureTracker::objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container) { // from some experimental testing 10 pixles is a good boarder to add around @@ -1182,7 +1209,7 @@ bool FeatureTracker::objectDetection( vision_tools::computeObjectMaskBoundaryMask( boundary_mask_result, object_mask, scaled_boarder_thickness, kUseAsFeatureDetectionMask); - return false; + return std::nullopt; } else { LOG(FATAL) << "Params specify prefer provided object mask but input " "is missing!"; @@ -1209,7 +1236,7 @@ bool FeatureTracker::objectDetection( // update or insert image container with object mask image_container.replace(ImageContainer::kObjectMask, object_mask); - return true; + return detection_result; } } @@ -1224,6 +1251,18 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // note reference cv::Mat& current_mask = image_container.objectMotionMask(); + cv::Mat& current_class_mask = image_container.objectClassMask(); + + // building map of previous classes + std::map object_classes; + for (const auto& detection : previous_frame->object_detection_.detections) { + if (detection.isValid()) { + object_classes[detection.obj_id] = detection.class_name; + } + else { + object_classes[detection.obj_id] = "Unknown"; + } + } ObjectIds instance_labels; for (const Feature::Ptr& dynamic_feature : @@ -1352,6 +1391,13 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { current_mask.at(functional_keypoint::v(predicted_kp), functional_keypoint::u(predicted_kp)) = instance_labels[i]; + + const ObjectId obj_id = instance_labels[i]; + int class_id = objectIdToClassId(obj_id, object_classes); + + current_class_mask.at(functional_keypoint::v(predicted_kp), + functional_keypoint::u(predicted_kp)) = + class_id; // current_rgb // updated_mask_points++; } @@ -1362,4 +1408,5 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { } } + } // namespace dyno diff --git a/dynosam_common/include/dynosam_common/DynamicObjects.hpp b/dynosam_common/include/dynosam_common/DynamicObjects.hpp index ef74681d6..82d33a61f 100644 --- a/dynosam_common/include/dynosam_common/DynamicObjects.hpp +++ b/dynosam_common/include/dynosam_common/DynamicObjects.hpp @@ -122,8 +122,10 @@ struct ObjectDetectionResult { * @param w_L_k_1 const gtsam::Pose3& * @return gtsam::Vector3 */ -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1); +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k); enum PropogateType { InitGT, diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index b136c7b2d..1b67c1e33 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -73,15 +73,48 @@ std::ostream& operator<<(std::ostream& os, return os; } -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1) { +// want this to also output angular velocity too +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k) { const gtsam::Point3& t_motion = w_k_1_H_k.translation(); const gtsam::Rot3& R_motion = w_k_1_H_k.rotation(); const gtsam::Point3& t_pose = w_L_k_1.translation(); + const gtsam::Rot3& R_pose = w_L_k_1.rotation(); static const gtsam::Rot3 I = gtsam::Rot3::Identity(); - return t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose; + Timestamp dt = 0.04; + + gtsam::Vector3 trans_vel = (t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose) / dt; + +// ===== NOW CALCULATING THE ANGULAR VELOCITY ===== + + // Finding the relative rotation over the timestep + gtsam::Rot3 R_pose_transpose = R_pose.inverse(); + gtsam::Rot3 R_new = R_motion * R_pose; + gtsam::Rot3 R_rel = R_pose_transpose * R_new; + + // Calculating the trace of the relative rotation and the angle of rotation + double trace = R_rel.matrix().trace(); + double angle = std::acos((trace - 1) / 2); + + gtsam::Matrix3 R_rel_m = R_rel.matrix(); + + // Finding the unit axis of rotation + double ux = (1 / (2 * std::sin(angle))) * (R_rel_m(2,1) - R_rel_m(1,2)); + double uy = (1 / (2 * std::sin(angle))) * (R_rel_m(0,2) - R_rel_m(2,0)); + double uz = (1 / (2 * std::sin(angle))) * (R_rel_m(1,0) - R_rel_m(0,1)); + const gtsam::Vector3 axis(ux, uy, uz); + + const gtsam::Vector3 angular_vel = (angle / dt) * axis; + + gtsam::Vector6 body_velocity; + body_velocity.head<3>() = trans_vel; + body_velocity.tail<3>() = angular_vel; + + return body_velocity; } void propogateObjectPoses(ObjectPoseMap& object_poses, diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index efdda9995..32d7959b8 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,6 +230,7 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; + static constexpr char kClassMask[] = "classmask" static constexpr char kRightRgb[] = "rightrgb"; public: @@ -306,24 +307,28 @@ class ImageContainer { inline bool hasDepth() const { return exists(kDepth); } inline bool hasOpticalFlow() const { return exists(kOPticalFlow); } inline bool hasObjectMask() const { return exists(kObjectMask); } + inline bool hasClassMask() const { return exists(kClassMask); } inline bool hasRightRgb() const { return exists(kRightRgb); } const ImageWrapper& rgb() const; const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; + const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); + ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); ImageContainer& depth(const cv::Mat& image); ImageContainer& opticalFlow(const cv::Mat& image); ImageContainer& objectMotionMask(const cv::Mat& image); + ImageContainer& objectClassMask(const cv::Mat& image); ImageContainer& rightRgb(const cv::Mat& image); Timestamp timestamp() const { return timestamp_; } diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index 2dca31a91..31c072bae 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -62,6 +62,12 @@ const ImageWrapper& ImageContainer::objectMotionMask() const { return this->at(kObjectMask); } + +const ImageWrapper& ImageContainer::objectClassMask() + const { + return this->at(kClassMask); + } + const ImageWrapper& ImageContainer::rightRgb() const { return this->at(kRightRgb); } @@ -78,6 +84,11 @@ ImageWrapper& ImageContainer::opticalFlow() { ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } + +ImageWrapper& ImageContainer::objectClassMask() { + return this->at(kClassMask); +} + ImageWrapper& ImageContainer::rightRgb() { return this->at(kRightRgb); } @@ -98,6 +109,10 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { return this->add(kObjectMask, image); } +ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { + return this->add(kClassMask, image); +} + ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { return this->add(kRightRgb, image); } diff --git a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp index 78f0f946e..daab0c1ce 100644 --- a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp @@ -68,7 +68,8 @@ struct DisplayCommon { static void publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id); + const std::string& child_frame_id, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); static void publishOdometryPath(PathPub::SharedPtr pub, const gtsam::Pose3Vector& poses, Timestamp latest_timestamp, diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp index d0e38aa15..a203455c3 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp @@ -83,12 +83,12 @@ class DSDTransport { // to get velocity... static ObjectOdometry constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link, const std::string& child_frame_id_link); static ObjectOdometryMap constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link); static MultiObjectOdometryPath constructMultiObjectOdometryPaths( @@ -146,6 +146,7 @@ class DSDTransport { std::string frame_id_link_; FrameId frame_id_; Timestamp timestamp_; + Timestamp timestamp_km1_; //! Object odometries for this frame ObjectOdometryMap object_odometries_; @@ -231,7 +232,8 @@ class DSDRos { DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node); void publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf); + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); void publishVisualOdometryPath(const gtsam::Pose3Vector& poses, Timestamp latest_timestamp); diff --git a/dynosam_ros/src/RosUtils.cc b/dynosam_ros/src/RosUtils.cc index 73d197092..6be26d1e6 100644 --- a/dynosam_ros/src/RosUtils.cc +++ b/dynosam_ros/src/RosUtils.cc @@ -158,6 +158,24 @@ bool dyno::convert(const gtsam::Pose3& pose, return true; } +// NEW! Template to add twist to the message +template <> +bool dyno::convert(const gtsam::Vector6& vel, + geometry_msgs::msg::Twist& twist) +{ + // linear velocity components + twist.linear.x = vel(0); + twist.linear.y = vel(1); + twist.linear.z = vel(2); + + // angular velocity components + twist.angular.x = vel(3); + twist.angular.y = vel(4); + twist.angular.z = vel(5); + + return true; +} + template <> bool dyno::convert(const gtsam::Pose3& pose, geometry_msgs::msg::TransformStamped& transform) { diff --git a/dynosam_ros/src/displays/DisplaysCommon.cc b/dynosam_ros/src/displays/DisplaysCommon.cc index 548523ef0..0cd0f9a2d 100644 --- a/dynosam_ros/src/displays/DisplaysCommon.cc +++ b/dynosam_ros/src/displays/DisplaysCommon.cc @@ -44,15 +44,23 @@ CloudPerObject DisplayCommon::publishPointCloud( return clouds_per_obj; } +// ================================================================================================================ +// TODO: look here edit this function to include velocities +// ================================================================================================================ + void DisplayCommon::publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id) { + const std::string& child_frame_id, + const gtsam::Vector6& velocity + ) { nav_msgs::msg::Odometry odom_msg; utils::convertWithHeader(T_world_camera, odom_msg, timestamp, frame_id, child_frame_id); - pub->publish(odom_msg); + + dyno::convert(velocity, odom_msg.twist.twist); + pub->publish(odom_msg); } void DisplayCommon::publishOdometryPath(PathPub::SharedPtr pub, diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc index 4624adf4b..5a180ee9a 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc @@ -25,24 +25,29 @@ std::string DSDTransport::constructObjectFrameLink(ObjectId object_id) { return "object_" + std::to_string(object_id) + "_link"; } +// ================================================================================================================ +// TODO: Fill in the velocity and check calculateBodyMotion is correct (only linear, add angular component too) +// ================================================================================================================ ObjectOdometry DSDTransport::constructObjectOdometry( - const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, + const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link, const std::string& child_frame_id_link) { ObjectOdometry object_odom; - // technically this shoudl be k-1 - gtsam::Point3 body_velocity = calculateBodyMotion(e_H_k_world, pose_k); + // technically this should be k-1 + gtsam::Vector6 body_velocity = calculateBodyMotion(e_H_k_world, pose_km1, timestamp_km1, timestamp_k); nav_msgs::msg::Odometry odom_msg; - utils::convertWithHeader(pose_k, odom_msg, timestamp_k, frame_id_link, + utils::convertWithHeader(pose_km1, odom_msg, timestamp_k, frame_id_link, child_frame_id_link); object_odom.odom = odom_msg; // TODO: can check if correct representation? dyno::convert(e_H_k_world, object_odom.h_w_km1_k.pose); - // NO velocity!! + + dyno::convert(body_velocity, object_odom.odom.twist.twist); // Add velocity to message + object_odom.object_id = object_id; object_odom.sequence = frame_id_k; return object_odom; @@ -50,7 +55,7 @@ ObjectOdometry DSDTransport::constructObjectOdometry( ObjectOdometryMap DSDTransport::constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link) { // need to get poses for k-1 // TODO: no way to ensure that the motions are for frame k @@ -79,8 +84,8 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( object_odom_map.insert2( child_frame_id_link, constructObjectOdometry(e_H_k_world, pose_k, object_id, frame_id_k, - timestamp_k, frame_id_link, - child_frame_id_link)); + timestamp_k, timestamp_km1, + frame_id_link, child_frame_id_link)); } return object_odom_map; @@ -139,9 +144,19 @@ MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( // RIGHT NOW MOTION IDENTITY // timestamp is wrong + + // Adding the (k-1)th timestamp + Timestamp timestamp_km1; + if (previous_frame_id >= 0) { + timestamp_km1 = frame_timestamp_map.at(previous_frame_id); + } + else { + timestamp_km1 = timestamp; + } + gtsam::Pose3 motion; const ObjectOdometry object_odometry = constructObjectOdometry( - object_motion, object_pose, object_id, frame_id, timestamp, + object_motion, object_pose, object_id, frame_id, timestamp, timestamp_km1, frame_id_link, child_frame_id_link); if (!segmented_paths.exists(path_segment)) { @@ -224,6 +239,23 @@ void DSDTransport::Publisher::publishObjectPaths() { multi_object_odom_path_publisher_->publish(object_paths_); } +// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTEP ============================== +Timestamp getPrevTimestamp(const FrameIdTimestampMap& map, FrameId id, Timestamp current_timestamp) { + + if (id < 0) return current_timestamp; + + FrameId prev_id = id - 1; + + auto it = map.find(prev_id); + if (it != map.end()) { + return it->second; + } + else { + return current_timestamp; + } +} +// ============================================================================================================ + DSDTransport::Publisher::Publisher( rclcpp::Node::SharedPtr node, ObjectOdometryPub::SharedPtr object_odom_publisher, @@ -240,8 +272,9 @@ DSDTransport::Publisher::Publisher( frame_id_link_(frame_id_link), frame_id_(frame_id), timestamp_(timestamp), + timestamp_km1_(getPrevTimestamp(frame_timestamp_map, frame_id, timestamp)), // added to get (k-1)th timestamp object_odometries_(DSDTransport::constructObjectOdometries( - motions, poses, frame_id, timestamp, frame_id_link)), + motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link)), object_paths_(DSDTransport::constructMultiObjectOdometryPaths( motions, poses, timestamp, frame_timestamp_map, frame_id_link)) {} @@ -271,10 +304,12 @@ DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) } void DSDRos::publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf) { + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity) { DisplayCommon::publishOdometry(vo_publisher_, T_world_camera, timestamp, params_.world_frame_id, - params_.camera_frame_id); + params_.camera_frame_id, + velocity); if (publish_tf) { geometry_msgs::msg::TransformStamped t; diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc index fe50cebfe..670eb9216 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc @@ -173,9 +173,12 @@ void FrontendDSDRos::tryPublishGroundTruth( void FrontendDSDRos::tryPublishVisualOdometry( const VisionImuPacket::ConstPtr& frontend_output) { // publish vo - constexpr static bool kPublishOdomAsTf = true; + constexpr static bool kPublishOdomAsTf = true; + this->publishVisualOdometry(frontend_output->cameraPose(), - frontend_output->timestamp(), kPublishOdomAsTf); + frontend_output->timestamp(), + kPublishOdomAsTf, + frontend_output->getBodyVelocity()); // relies on correct accumulation of internal objects this->publishVisualOdometryPath(camera_poses_, frontend_output->timestamp()); diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py index 8af56e3d5..906b4526a 100644 --- a/dynosam_utils/src/run_experiments_ecmr.py +++ b/dynosam_utils/src/run_experiments_ecmr.py @@ -33,7 +33,6 @@ def run_sequnce(path, name, data_loader_num, backend_type, *args, **kwargs): parsed_args["launch_file"] = "dyno_sam_launch.py" - if run_as_frontend: additional_args.extend([ "--use_backend=0", @@ -155,13 +154,13 @@ def run_viodes(): # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/mid", "viode_city_day_mid", viode, "--v=100") # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","viode_city_day_high", viode, "--ending_frame=1110") - run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode1", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") # # zero_elements_ratio # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/mid", "viode_city_night_mid", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/high", "viode_city_night_high", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/mid", "parking_lot_night_mid", viode) - # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/high", "parking_lot_night_high", viode) + run_ecmr_experiment_sequences("/root/data/viode_parking_lot/high", "parking_lot_night_high", viode) def run_omd(): run_ecmr_experiment_sequences("/root/data/vdo_slam/omd/omd/swinging_4_unconstrained_stereo/","test", omd_dataset, "--ending_frame=300") @@ -173,7 +172,7 @@ def run_tartan_air(): run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing05", "test_tartan", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing06", "tas_rc6", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing07", "tas_rc7", tartan_air, "--starting_frame=5", "--ending_frame=65") - # run_analysis("tas_rc7") + # run_analysis("tas_rc7") # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing01", "tas_s1", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing02", "tas_s2", tartan_air) @@ -192,8 +191,8 @@ def run_kitti(): # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0005/", "kitti_0005_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0006/", "kitti_0006_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0018/", "kitti_0018_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0004/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + run_ecmr_experiment_sequences("/root/data/kitti_0001/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") def run_aria(): @@ -207,10 +206,10 @@ def run_aria(): # "--ending_frame=150" # ) - # run_viodes() + run_viodes() # run_tartan_air() # run_cluster() - run_omd() + # run_omd() # run_aria() # run_kitti() # run_analysis("kitti_0001_static_only") @@ -499,4 +498,4 @@ def run_aria(): # "kitti_0003", # backend_type=object_centric_batch # ) - # run_analysis("kitti_0003") + # run_analysis("kitti_0003") \ No newline at end of file From 3adde14786e3c6bedc291a344c7342ad9a8f4fcd Mon Sep 17 00:00:00 2001 From: Ethan Date: Wed, 17 Dec 2025 16:28:43 +1100 Subject: [PATCH 02/13] Smaller fixes --- dynosam/src/frontend/vision/FeatureTracker.cc | 22 +++++++++---------- .../include/dynosam_cv/ImageContainer.hpp | 6 ++--- dynosam_cv/src/ImageContainer.cc | 10 ++++----- dynosam_utils/src/run_experiments_ecmr.py | 4 ++-- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index 888d80a00..de8a7c10c 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -48,19 +48,19 @@ namespace dyno { // HELPER FUNCTION ADDED BY ETHAN -// map for classes to number IDs (so we can expand later) -static const std::map class_name_to_id{ - {"Undefined", ClassSegmentation::Undefined}, // 0 - {"Road", ClassSegmentation::Road}, // 1 - {"Rider", ClassSegmentation::Rider}, // 2 +// TODO: make this map for the class labels +static const std::map class_name_to_id{ + {"Undefined", 0}, // 0 + {"Road", 1}, // 1 + {"Rider", 2}, // 2 }; int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { auto it = object_classes.find(obj_id); // find the object in the map - if (it == object_classes.end()) { - return ClassSegmentation::Undefined; + if (it == object_classes.end() || it->second == "Unknown") { + return 0; } auto class_it = class_name_to_id.find(it->second); @@ -68,7 +68,7 @@ int objectIdToClassId(ObjectId obj_id, const std::map& ob return class_it->second; } - return ClassSegmentation::Undefined; + return 1; } @@ -1255,12 +1255,12 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // building map of previous classes std::map object_classes; - for (const auto& detection : previous_frame->object_detection_.detections) { + for (const auto& [obj_id, detection] : previous_frame_->object_observations_) { if (detection.isValid()) { - object_classes[detection.obj_id] = detection.class_name; + object_classes[obj_id] = detection.class_name; } else { - object_classes[detection.obj_id] = "Unknown"; + object_classes[obj_id] = "Unknown"; } } diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index 32d7959b8..17034a064 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,7 +230,7 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; - static constexpr char kClassMask[] = "classmask" + static constexpr char kClassMask[] = "classmask"; static constexpr char kRightRgb[] = "rightrgb"; public: @@ -314,14 +314,14 @@ class ImageContainer { const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; - const ImageWrapper& objectClassMask() const; + const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); - ImageWrapper& objectClassMask(); + ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index 31c072bae..efefe2e3f 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -63,9 +63,9 @@ const ImageWrapper& ImageContainer::objectMotionMask() return this->at(kObjectMask); } -const ImageWrapper& ImageContainer::objectClassMask() +const ImageWrapper& ImageContainer::objectClassMask() const { - return this->at(kClassMask); + return this->at(kClassMask); } const ImageWrapper& ImageContainer::rightRgb() const { @@ -85,8 +85,8 @@ ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } -ImageWrapper& ImageContainer::objectClassMask() { - return this->at(kClassMask); +ImageWrapper& ImageContainer::objectClassMask() { + return this->at(kClassMask); } ImageWrapper& ImageContainer::rightRgb() { @@ -110,7 +110,7 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { } ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { - return this->add(kClassMask, image); + return this->add(kClassMask, image); } ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py index 906b4526a..e115f1b5e 100644 --- a/dynosam_utils/src/run_experiments_ecmr.py +++ b/dynosam_utils/src/run_experiments_ecmr.py @@ -206,12 +206,12 @@ def run_aria(): # "--ending_frame=150" # ) - run_viodes() + # run_viodes() # run_tartan_air() # run_cluster() # run_omd() # run_aria() - # run_kitti() + run_kitti() # run_analysis("kitti_0001_static_only") # run_analysis("kitti_0002_static_only") # run_analysis("kitti_0003_static_only") From 570d1f9ebec5763529bb29cc5448595d10ab60b6 Mon Sep 17 00:00:00 2001 From: fmuehlis Date: Mon, 19 Jan 2026 10:04:11 +0100 Subject: [PATCH 03/13] fix: Dockerfile not building due to missing apt-get update --- docker/Dockerfile.amd64 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile.amd64 b/docker/Dockerfile.amd64 index 7d0806905..77aa1b4cc 100644 --- a/docker/Dockerfile.amd64 +++ b/docker/Dockerfile.amd64 @@ -30,7 +30,7 @@ RUN apt-get update \ && rm -rf /var/lib/apt/lists/* # ROS installs -RUN apt-get install -y \ +RUN apt-get update && apt-get install -y \ ros-kilted-ros2cli-common-extensions \ ros-kilted-vision-opencv \ nlohmann-json3-dev From 829cc06adc89bd73d49bca8537d2787a34424584 Mon Sep 17 00:00:00 2001 From: fmuehlis Date: Mon, 19 Jan 2026 10:17:11 +0100 Subject: [PATCH 04/13] fix: MPI_C not being found during build inside Docker --- dynosam_nn/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dynosam_nn/CMakeLists.txt b/dynosam_nn/CMakeLists.txt index 8f1104308..8d40b3c6f 100644 --- a/dynosam_nn/CMakeLists.txt +++ b/dynosam_nn/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.17 FATAL_ERROR) -project(dynosam_nn LANGUAGES CXX CUDA) +project(dynosam_nn LANGUAGES C CXX CUDA) set(CMAKE_C_FLAGS "-Wall -Wextra -O3") @@ -12,6 +12,7 @@ set(CMAKE_CXX_STANDARD 17) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +find_package(MPI REQUIRED) find_package(dynosam_common REQUIRED) find_package(dynosam_cv REQUIRED) find_package(cv_bridge REQUIRED) From 33f720603636738927b7ddcf1884eb1df540dbc5 Mon Sep 17 00:00:00 2001 From: fmuehlis Date: Mon, 19 Jan 2026 10:17:24 +0100 Subject: [PATCH 05/13] fix: typos and some formatting --- README.md | 2 +- docker/README.md | 10 +++++++--- docs/media/INSTALL.md | 4 ++-- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 5e1f6aa98..e898f063e 100644 --- a/README.md +++ b/README.md @@ -92,7 +92,7 @@ We currently support building for AARM64 devices and DynoSAM has been tested on > NOTE: DynoSAM does not currently run real-time on the ORIN NX (mostly bottlenecked by the object detection process). On a more powerful device better performance is expected. -Also see the [Docker REAMDE.md](./docker/README.md) and the [dynosam_nn README.md](./dynosam_nn//README.md) for more information on hardware and performance. +Also see the [Docker README.md](./docker/README.md) and the [dynosam_nn README.md](./dynosam_nn//README.md) for more information on hardware and performance. # 2. 🚀 Running DynoSAM diff --git a/docker/README.md b/docker/README.md index 4db1cbc4e..3a7c70fc0 100644 --- a/docker/README.md +++ b/docker/README.md @@ -2,10 +2,11 @@ Base images are pulled from [docker-ros-ml-images](https://github.com/ika-rwth-aachen/docker-ros-ml-images) -- Dockerfile.amd64 is a linux/amd64 image tested on x86_64 desktop -- Dockerfile.l4t_jetpack6 is build from linux/arm64 tested on an NVIDIA ORIN NX with Jetpack 6 +- `Dockerfile.amd64` is a linux/amd64 image tested on x86_64 desktop +- `Dockerfile.l4t_jetpack6` is build from linux/arm64 tested on an NVIDIA ORIN NX with Jetpack 6 ## Jetson Settings +``` Architecture | aarch64 Ubuntu | 22.04.5 LTS (Jammy Jellyfish) Jetson Linux | 36.4.7 @@ -18,9 +19,12 @@ TensorRT | 10.7.0.23-1+cuda12.6 PyTorch | 2.8.0 GPUs | (Orin (nvgpu)) OpenCV | 4.10.0 +``` > NOTE: The CUDA/Pytorch/TensorRT versions settings come with the base dockerfile but in practice we have been using CUDA 12.9. ## Other versioning +```txt matplotlib=3.6.3 -numpy=1.26.4 \ No newline at end of file +numpy=1.26.4 +``` \ No newline at end of file diff --git a/docs/media/INSTALL.md b/docs/media/INSTALL.md index dca9a197f..5d43fd175 100644 --- a/docs/media/INSTALL.md +++ b/docs/media/INSTALL.md @@ -29,9 +29,9 @@ To install natively, install the dependancies as required by docker and build as ## Docker Install instructions -DynoSAM has been tested on __x86_64__ and __aarm64__ (with a NVIDIA ORIN NX) devices using the [two docker files](../../docker/) provided. See the [REAMDE.md](../../docker/README.md) for more detail on hardware used etc. +DynoSAM has been tested on __x86_64__ and __aarm64__ (with a NVIDIA ORIN NX) devices using the [two docker files](../../docker/) provided. See the [README.md](../../docker/README.md) for more detail on hardware used etc. -We provide scripts to build and create docker containers to run and develop DynoSAM which is intendend to be mounted within the created container. +We provide scripts to build and create docker containers to run and develop DynoSAM which is intended to be mounted within the created container. > NOTE: with the current setup the embedded device only supports ROS Jazzy. The code should compile on either device without modification. From ee66d6ec07094f1dc10b4b84cf0e5d0dd37ee66e Mon Sep 17 00:00:00 2001 From: fmuehlis Date: Mon, 19 Jan 2026 10:48:26 +0100 Subject: [PATCH 06/13] fix: typos and formatting in dynosam_nn/README.md --- dynosam_nn/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dynosam_nn/README.md b/dynosam_nn/README.md index 3aacbdefe..ed8af5409 100644 --- a/dynosam_nn/README.md +++ b/dynosam_nn/README.md @@ -19,14 +19,14 @@ For object-level tracking we use a modified C++ implementation of ByteTracker > NOTE: this is the _install_ directory (ie. in the docker container it will be `/home/user/dev_ws/install/dynosam_nn/share/dynosam_nn/weights/`) - To export the model navigate to `dynosam_nn/export` and run ``` -run python3 export_yolo_tensorrt.py +python3 export_yolo_tensorrt.py ``` which should export a `.onnx` model to the weights directory. - YoloV8ObjectDetector requires an exported `.onnx` file which will be converted to a `.engine` file when first loaded. ## Install -- python3 -m pip install "ultralytics==8.3.0" "numpy<2.0" "opencv-python<5.0" -- sudo apt install python3-pybind11 +- `python3 -m pip install "ultralytics==8.3.0" "numpy<2.0" "opencv-python<5.0"` +- `sudo apt install python3-pybind11` > NOTE: these dependancies should already be installed when using the Dockerfile From 8c20caa1e6b08428d49e687a5f866c1a2b960f51 Mon Sep 17 00:00:00 2001 From: Jesse Morris <34204481+jessemorris@users.noreply.github.com> Date: Fri, 23 Jan 2026 08:30:40 +0100 Subject: [PATCH 07/13] Updated readme with RA-L Updated publication details and added new acceptance information for papers. --- README.md | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index e898f063e..5b7e53b66 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,11 @@ ## 📚 Publications The offical code used for our paper: -- [Jesse Morris](https://jessemorris.github.io/), Yiduo Wang, Mikolaj Kliniewski, Viorela Ila, [*DynoSAM: Open-Source Smoothing and Mapping Framework for Dynamic SLAM*](https://arxiv.org/pdf/2501.11893). Accepted Transactions on Robotics (T-RO) Visual SLAM Special Issue (2025). +- [Jesse Morris](https://jessemorris.github.io/), Yiduo Wang, Mikolaj Kliniewski, Viorela Ila, [*DynoSAM: Open-Source Smoothing and Mapping Framework for Dynamic SLAM*](https://arxiv.org/pdf/2501.11893). Accepted Transactions on Robotics (T-RO) Visual SLAM Special Issue, 2025. + + +#### ** Update December 2025 ** +Our work has been accepted to IEEE Robotics and Automation Letters (RA-L) #### ** Update November 2025 ** Our work has been accepted to IEEE Transactions on Robotics (T-RO) @@ -37,7 +41,7 @@ Our work has been accepted to IEEE Transactions on Robotics (T-RO) #### ** Update September 2025 ** This code now also contains the code for our new work -- J.Morris, Y. Wang, V. Ila. [*Online Dynamic SLAM with Incremental Smoothing and Mapping*](https://www.arxiv.org/abs/2509.08197), Arxiv. Submitted RA-L (2025) +- J.Morris, Y. Wang, V. Ila. [*Online Dynamic SLAM with Incremental Smoothing and Mapping*](https://www.arxiv.org/abs/2509.08197), Accepted Robotics and Automation Letters (RA-L), 2025 We kindly ask to cite our papers if you find these works useful: @@ -53,11 +57,15 @@ We kindly ask to cite our papers if you find these works useful: doi={10.1109/TRO.2025.3641813} } -@article{morris2025online, - title={Online Dynamic SLAM with Incremental Smoothing and Mapping}, +@inproceedings{morris2025online, author={Morris, Jesse and Wang, Yiduo and Ila, Viorela}, - journal={arXiv preprint arXiv:2509.08197}, - year={2025} + journal={IEEE Robotics and Automation Letters}, + title={Online Dynamic SLAM with Incremental Smoothing and Mapping}, + year={2026}, + volume={}, + number={}, + pages={1-8}, + doi={10.1109/LRA.2026.3655286}} } ``` From cc879c10c6324d7222a468c6583c019c5835fd06 Mon Sep 17 00:00:00 2001 From: Ethan Date: Thu, 29 Jan 2026 14:46:22 +1100 Subject: [PATCH 08/13] Added semantic label passthrough --- .../dynosam/frontend/FrontendModule.hpp | 4 + .../frontend/vision/FeatureTracker.hpp | 4 +- dynosam/src/frontend/FrontendModule.cc | 14 + .../frontend/RGBDInstanceFrontendModule.cc | 4 + dynosam/src/frontend/vision/FeatureTracker.cc | 90 ++-- .../dynosam_common/SharedModuleInfo.hpp | 3 + .../include/dynosam_common/Types.hpp | 3 + dynosam_common/src/DynamicObjects.cc | 23 +- dynosam_common/src/SharedModuleInfo.cc | 18 + .../include/dynosam_cv/ImageContainer.hpp | 5 - dynosam_cv/src/ImageContainer.cc | 12 - .../dynamic_slam_displays/DSDCommonRos.hpp | 17 +- .../dynamic_slam_displays/DSDCommonRos.cc | 41 +- .../dynamic_slam_displays/FrontendDSDRos.cc | 3 +- .../src/install/.colcon_install_layout | 1 + dynosam_utils/src/install/COLCON_IGNORE | 0 .../src/install/_local_setup_util_ps1.py | 407 ++++++++++++++++++ .../src/install/_local_setup_util_sh.py | 407 ++++++++++++++++++ dynosam_utils/src/install/local_setup.bash | 121 ++++++ dynosam_utils/src/install/local_setup.ps1 | 55 +++ dynosam_utils/src/install/local_setup.sh | 137 ++++++ dynosam_utils/src/install/local_setup.zsh | 134 ++++++ dynosam_utils/src/install/setup.bash | 34 ++ dynosam_utils/src/install/setup.ps1 | 30 ++ dynosam_utils/src/install/setup.sh | 49 +++ dynosam_utils/src/install/setup.zsh | 34 ++ dynosam_utils/src/log/COLCON_IGNORE | 0 .../log/build_2026-01-27_04-03-52/events.log | 2 + .../build_2026-01-27_04-03-52/logger_all.log | 64 +++ .../log/build_2026-01-28_00-38-18/events.log | 2 + .../build_2026-01-28_00-38-18/logger_all.log | 65 +++ dynosam_utils/src/log/latest | 1 + dynosam_utils/src/log/latest_build | 1 + dynosam_utils/src/run_experiments_ecmr.py | 0 34 files changed, 1690 insertions(+), 95 deletions(-) create mode 100644 dynosam_utils/src/install/.colcon_install_layout create mode 100644 dynosam_utils/src/install/COLCON_IGNORE create mode 100644 dynosam_utils/src/install/_local_setup_util_ps1.py create mode 100644 dynosam_utils/src/install/_local_setup_util_sh.py create mode 100644 dynosam_utils/src/install/local_setup.bash create mode 100644 dynosam_utils/src/install/local_setup.ps1 create mode 100644 dynosam_utils/src/install/local_setup.sh create mode 100644 dynosam_utils/src/install/local_setup.zsh create mode 100644 dynosam_utils/src/install/setup.bash create mode 100644 dynosam_utils/src/install/setup.ps1 create mode 100644 dynosam_utils/src/install/setup.sh create mode 100644 dynosam_utils/src/install/setup.zsh create mode 100644 dynosam_utils/src/log/COLCON_IGNORE create mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log create mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log create mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log create mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log create mode 120000 dynosam_utils/src/log/latest create mode 120000 dynosam_utils/src/log/latest_build mode change 100644 => 100755 dynosam_utils/src/run_experiments_ecmr.py diff --git a/dynosam/include/dynosam/frontend/FrontendModule.hpp b/dynosam/include/dynosam/frontend/FrontendModule.hpp index 018588e51..1cbc140cc 100644 --- a/dynosam/include/dynosam/frontend/FrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/FrontendModule.hpp @@ -39,6 +39,8 @@ #include "dynosam_common/ModuleBase.hpp" #include "dynosam_common/SharedModuleInfo.hpp" #include "dynosam_common/Types.hpp" +#include "dynosam/frontend/vision/FeatureTracker.hpp" + // #include "dynosam_common" @@ -84,6 +86,8 @@ class FrontendModule virtual void onBackendUpdateCallback(const FrameId /*frame_id*/, const Timestamp /*timestamp*/) {} + void FrameToClassMap(const Frame::Ptr& frame) const; + protected: /** * @brief Defines the result of checking the image container which is a done diff --git a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp index 7391d93da..7a8ee78b7 100644 --- a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp +++ b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp @@ -36,10 +36,12 @@ #include "dynosam/frontend/FrontendParams.hpp" #include "dynosam/frontend/vision/FeatureTrackerBase.hpp" #include "dynosam/frontend/vision/Frame.hpp" +#include "dynosam/frontend/FrontendModule.hpp" #include "dynosam/frontend/vision/StaticFeatureTracker.hpp" #include "dynosam/visualizer/Visualizer-Definitions.hpp" #include "dynosam_cv/Camera.hpp" #include "dynosam_cv/Feature.hpp" +#include "dynosam_common/SharedModuleInfo.hpp" // #include "dynosam_common/DynamicObjects.hpp" #include "dynosam_nn/ObjectDetector.hpp" @@ -146,7 +148,7 @@ class FeatureTracker : public FeatureTrackerBase { private: // TODO: for now we loose the actual object detection result if inference was // run! - std::optional objectDetection( + ObjectDetectionResult objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container); diff --git a/dynosam/src/frontend/FrontendModule.cc b/dynosam/src/frontend/FrontendModule.cc index e084817d3..0bfaf3b0c 100644 --- a/dynosam/src/frontend/FrontendModule.cc +++ b/dynosam/src/frontend/FrontendModule.cc @@ -66,4 +66,18 @@ void FrontendModule::validateInput( result.valid_, *input->image_container_, result.requirement_); } +void FrontendModule::FrameToClassMap(const Frame::Ptr& frame) const { + const auto& object_observations = frame->getObjectObservations(); + + for (const auto& [object_id, detection] : object_observations) { + if (!detection.class_name.empty()) { + shared_module_info.updateClassLabelMapping(object_id, detection.class_name); + std::cout << "UPDATING CLASS LABEL MAPPING " << object_id << " IS " << detection.class_name << std::endl; + } + else { + std::cout << "CANNOT UPDATE CLASS LABEL MAPPING; IT WAS EMPTY!" << std::endl; + } + } +} + } // namespace dyno diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index 50fff5fac..ed886af18 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -116,6 +116,9 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::boostrapSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container); + FrontendModule::FrameToClassMap(frame); + + CHECK(frame->updateDepths()); return {State::Nominal, nullptr}; @@ -148,6 +151,7 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container, R_curr_ref); + FrontendModule::FrameToClassMap(frame); Frame::Ptr previous_frame = tracker_->getPreviousFrame(); CHECK(previous_frame); diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index de8a7c10c..8e0278436 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -47,32 +47,6 @@ namespace dyno { - // HELPER FUNCTION ADDED BY ETHAN -// TODO: make this map for the class labels -static const std::map class_name_to_id{ - {"Undefined", 0}, // 0 - {"Road", 1}, // 1 - {"Rider", 2}, // 2 -}; - -int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { - - auto it = object_classes.find(obj_id); // find the object in the map - - if (it == object_classes.end() || it->second == "Unknown") { - return 0; - } - - auto class_it = class_name_to_id.find(it->second); - if (class_it != class_name_to_id.end()) { - return class_it->second; - } - - return 1; - -} - - FeatureTracker::FeatureTracker(const FrontendParams& params, Camera::Ptr camera, ImageDisplayQueue* display_queue) : FeatureTrackerBase(params.tracker_params, camera, display_queue), @@ -129,7 +103,12 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // params_.prefer_provided_object_detection is false vision_tools::ObjectBoundaryMaskResult boundary_mask_result; - objectDetection(boundary_mask_result, input_images); + ObjectDetectionResult observations = objectDetection(boundary_mask_result, input_images); + + for (const auto& det : observations.detections) { + std::cout << "Detected object_id=" << det.object_id + << " class_name=" << det.class_name << std::endl; + } if (!initial_computation_ && params_.use_propogate_mask) { utils::TimingStatsCollector timer("propogate_mask"); @@ -183,18 +162,10 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // TODO: SingleDetectionResult really does not need the tracklet ids they // are never actually used!! this prevents the frame from needing to do the // same calculations we've alrady done - std::map object_observations; - for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { - ObjectId object_id = boundary_mask_result.objects_detected.at(i); - const cv::Rect& bb_detection = - boundary_mask_result.object_bounding_boxes.at(i); - - SingleDetectionResult observation; - observation.object_id = object_id; - // observation.object_features = dynamic_features.getByObject(object_id); - observation.bounding_box = bb_detection; - object_observations[object_id] = observation; + std::map object_observations; + for (const auto& detection : observations.detections) { + object_observations[detection.object_id] = detection; } utils::TimingStatsCollector f_timer("tracking_timer.frame_construction"); @@ -216,6 +187,8 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, previous_frame_ = new_frame; boarder_detection_mask_ = boundary_mask_result.boundary_mask; + // FrontendModule::FrameToClassMap(new_frame); + return new_frame; } @@ -1178,7 +1151,7 @@ void FeatureTracker::requiresSampling( } } -std::optional FeatureTracker::objectDetection( +ObjectDetectionResult FeatureTracker::objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container) { // from some experimental testing 10 pixles is a good boarder to add around @@ -1209,7 +1182,25 @@ std::optional FeatureTracker::objectDetection( vision_tools::computeObjectMaskBoundaryMask( boundary_mask_result, object_mask, scaled_boarder_thickness, kUseAsFeatureDetectionMask); - return std::nullopt; + + ObjectDetectionResult detection_result; + detection_result.input_image = image_container.rgb(); + detection_result.labelled_mask = object_mask; + + for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { + ObjectId object_id = boundary_mask_result.objects_detected.at(i); + const cv::Rect& bb_detection = + boundary_mask_result.object_bounding_boxes.at(i); + + SingleDetectionResult observation; + observation.object_id = object_id; + // observation.object_features = dynamic_features.getByObject(object_id); + observation.bounding_box = bb_detection; + observation.class_name = "goober"; + detection_result.detections.push_back(observation); + } + + return detection_result; } else { LOG(FATAL) << "Params specify prefer provided object mask but input " "is missing!"; @@ -1251,18 +1242,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // note reference cv::Mat& current_mask = image_container.objectMotionMask(); - cv::Mat& current_class_mask = image_container.objectClassMask(); - - // building map of previous classes - std::map object_classes; - for (const auto& [obj_id, detection] : previous_frame_->object_observations_) { - if (detection.isValid()) { - object_classes[obj_id] = detection.class_name; - } - else { - object_classes[obj_id] = "Unknown"; - } - } + ObjectIds instance_labels; for (const Feature::Ptr& dynamic_feature : @@ -1391,13 +1371,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { current_mask.at(functional_keypoint::v(predicted_kp), functional_keypoint::u(predicted_kp)) = instance_labels[i]; - - const ObjectId obj_id = instance_labels[i]; - int class_id = objectIdToClassId(obj_id, object_classes); - current_class_mask.at(functional_keypoint::v(predicted_kp), - functional_keypoint::u(predicted_kp)) = - class_id; // current_rgb // updated_mask_points++; } diff --git a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp index f88d28c2d..a7648b025 100644 --- a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp +++ b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp @@ -43,6 +43,7 @@ class SharedModuleInfo { std::optional getGroundTruthPackets() const; const FrameIdTimestampMap& getTimestampMap() const; + const ObjectIdClassMap& getClassLabelMap() const; bool getTimestamp(FrameId frame_id, Timestamp& timestamp) const; @@ -50,6 +51,7 @@ class SharedModuleInfo { FrameId frame_id, const GroundTruthInputPacket& ground_truth_packet); SharedModuleInfo& updateTimestampMapping(FrameId frame_id, Timestamp timestamp); + SharedModuleInfo& updateClassLabelMapping(ObjectId object_id, std::string class_label); private: static std::unique_ptr instance_; @@ -58,6 +60,7 @@ class SharedModuleInfo { mutable std::mutex mutex_; GroundTruthPacketMap gt_packet_map_; FrameIdTimestampMap frame_id_to_timestamp_map_; + ObjectIdClassMap object_id_to_class_map_; }; struct SharedModuleInterface { diff --git a/dynosam_common/include/dynosam_common/Types.hpp b/dynosam_common/include/dynosam_common/Types.hpp index d19bbe26d..9a764b1d3 100644 --- a/dynosam_common/include/dynosam_common/Types.hpp +++ b/dynosam_common/include/dynosam_common/Types.hpp @@ -111,6 +111,9 @@ using MotionMap = /// @brief Map of FrameIds (k) to Timestamps using FrameIdTimestampMap = gtsam::FastMap; +/// @brief Map of Object Ids to CLass Labels +using ObjectIdClassMap = gtsam::FastMap; + // T is expected to have (at least) bitwise | (OR) support template class Flags { diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index 1b67c1e33..148cbd50c 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -73,6 +73,8 @@ std::ostream& operator<<(std::ostream& os, return os; } + +// OLD BODY MOTION CALCULATION IMPLEMENTATION // want this to also output angular velocity too gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, const gtsam::Pose3& w_L_k_1, @@ -85,7 +87,10 @@ gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, static const gtsam::Rot3 I = gtsam::Rot3::Identity(); - Timestamp dt = 0.04; + double dt = timestamp_k - timestamp_km1; + if (dt <= 1e-6) { + dt = 0.1; // fallback to nominal frame interval + } gtsam::Vector3 trans_vel = (t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose) / dt; @@ -117,6 +122,22 @@ gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, return body_velocity; } +// gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, +// const gtsam::Pose3& w_L_k_1, +// Timestamp timestamp_km1, +// Timestamp timestamp_k) { + +// // Finding relative pose +// Timestamp dt = 0.04; // Estimated time between frames in kitti datasets + +// gtsam::Pose3 T_rel = w_L_k_1.inverse() * (w_k_1_H_k * w_L_k_1); // if pose is given in world frame +// // gtsam::Pose3 T_rel = w_k_1_H_k; // if pose is already in body frame + +// gtsam::Vector6 body_velocity = gtsam::Pose3::Logmap(T_rel) / dt; + +// return body_velocity; +// } + void propogateObjectPoses(ObjectPoseMap& object_poses, const MotionEstimateMap& object_motions_k, const gtsam::Point3Vector& object_centroids_k_1, diff --git a/dynosam_common/src/SharedModuleInfo.cc b/dynosam_common/src/SharedModuleInfo.cc index 076b86213..c6c966a0e 100644 --- a/dynosam_common/src/SharedModuleInfo.cc +++ b/dynosam_common/src/SharedModuleInfo.cc @@ -57,6 +57,11 @@ std::optional SharedModuleInfo::getGroundTruthPackets() return gt_packet_map_; } +const ObjectIdClassMap& SharedModuleInfo::getClassLabelMap() const { + const std::lock_guard lock(mutex_); + return object_id_to_class_map_; +} + bool SharedModuleInfo::getTimestamp(FrameId frame_id, Timestamp& timestamp) const { const FrameIdTimestampMap& timestamp_map = getTimestampMap(); @@ -88,4 +93,17 @@ SharedModuleInfo& SharedModuleInfo::updateTimestampMapping( return *this; } +SharedModuleInfo& SharedModuleInfo::updateClassLabelMapping( + ObjectId object_id, std::string class_label) { + + const std::lock_guard lock(mutex_); + + if (object_id_to_class_map_.exists(object_id)) { + CHECK_EQ(object_id_to_class_map_.at(object_id), class_label); + } else { + object_id_to_class_map_.insert2(object_id, class_label); + } + return *this; +} + } // namespace dyno diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index 17034a064..efdda9995 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,7 +230,6 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; - static constexpr char kClassMask[] = "classmask"; static constexpr char kRightRgb[] = "rightrgb"; public: @@ -307,28 +306,24 @@ class ImageContainer { inline bool hasDepth() const { return exists(kDepth); } inline bool hasOpticalFlow() const { return exists(kOPticalFlow); } inline bool hasObjectMask() const { return exists(kObjectMask); } - inline bool hasClassMask() const { return exists(kClassMask); } inline bool hasRightRgb() const { return exists(kRightRgb); } const ImageWrapper& rgb() const; const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; - const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); - ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); ImageContainer& depth(const cv::Mat& image); ImageContainer& opticalFlow(const cv::Mat& image); ImageContainer& objectMotionMask(const cv::Mat& image); - ImageContainer& objectClassMask(const cv::Mat& image); ImageContainer& rightRgb(const cv::Mat& image); Timestamp timestamp() const { return timestamp_; } diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index efefe2e3f..91de1336a 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -63,10 +63,6 @@ const ImageWrapper& ImageContainer::objectMotionMask() return this->at(kObjectMask); } -const ImageWrapper& ImageContainer::objectClassMask() - const { - return this->at(kClassMask); - } const ImageWrapper& ImageContainer::rightRgb() const { return this->at(kRightRgb); @@ -85,10 +81,6 @@ ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } -ImageWrapper& ImageContainer::objectClassMask() { - return this->at(kClassMask); -} - ImageWrapper& ImageContainer::rightRgb() { return this->at(kRightRgb); } @@ -109,10 +101,6 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { return this->add(kObjectMask, image); } -ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { - return this->add(kClassMask, image); -} - ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { return this->add(kRightRgb, image); } diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp index a203455c3..be8719665 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp @@ -84,18 +84,21 @@ class DSDTransport { static ObjectOdometry constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link, const std::string& child_frame_id_link); + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map); static ObjectOdometryMap constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link); + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map); static MultiObjectOdometryPath constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, const std::string& frame_id_link, - bool interpolate_missing_segments = true); + bool interpolate_missing_segments = true, + const ObjectIdClassMap& object_class_map = {}); /** * @brief Nested Publisher that publishes all the object odometries for a @@ -152,6 +155,8 @@ class DSDTransport { ObjectOdometryMap object_odometries_; MultiObjectOdometryPath object_paths_; + ObjectIdClassMap object_class_map_; + friend class DSDTransport; /** @@ -181,7 +186,8 @@ class DSDTransport { const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp); + Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); }; /** @@ -202,7 +208,8 @@ class DSDTransport { const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, - FrameId frame_id, Timestamp timestamp); + FrameId frame_id, Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); private: rclcpp::Node::SharedPtr node_; diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc index 5a180ee9a..0cf068766 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc @@ -31,7 +31,8 @@ std::string DSDTransport::constructObjectFrameLink(ObjectId object_id) { ObjectOdometry DSDTransport::constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link, const std::string& child_frame_id_link) { + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map) { ObjectOdometry object_odom; // technically this should be k-1 @@ -50,13 +51,24 @@ ObjectOdometry DSDTransport::constructObjectOdometry( object_odom.object_id = object_id; object_odom.sequence = frame_id_k; + + // Assigning the class label + auto observation = object_class_map.find(object_id); + if (observation != object_class_map.end()) { + object_odom.class_label=observation->second; + } else { + object_odom.class_label="unknown"; + } + + return object_odom; } ObjectOdometryMap DSDTransport::constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link) { + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map) { // need to get poses for k-1 // TODO: no way to ensure that the motions are for frame k // this is a weird data-structure to use and motions are per frame and @@ -85,7 +97,7 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( child_frame_id_link, constructObjectOdometry(e_H_k_world, pose_k, object_id, frame_id_k, timestamp_k, timestamp_km1, - frame_id_link, child_frame_id_link)); + frame_id_link, child_frame_id_link, object_class_map)); } return object_odom_map; @@ -94,7 +106,7 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, - const std::string& frame_id_link, bool interpolate_missing_segments) { + const std::string& frame_id_link, bool interpolate_missing_segments, const ObjectIdClassMap& object_class_map) { MultiObjectOdometryPath multi_path; multi_path.header.stamp = utils::toRosTime(timestamp_k); @@ -157,7 +169,7 @@ MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( gtsam::Pose3 motion; const ObjectOdometry object_odometry = constructObjectOdometry( object_motion, object_pose, object_id, frame_id, timestamp, timestamp_km1, - frame_id_link, child_frame_id_link); + frame_id_link, child_frame_id_link, object_class_map); if (!segmented_paths.exists(path_segment)) { ObjectOdometryPath path; @@ -239,7 +251,7 @@ void DSDTransport::Publisher::publishObjectPaths() { multi_object_odom_path_publisher_->publish(object_paths_); } -// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTEP ============================== +// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTAMP ============================== Timestamp getPrevTimestamp(const FrameIdTimestampMap& map, FrameId id, Timestamp current_timestamp) { if (id < 0) return current_timestamp; @@ -264,7 +276,8 @@ DSDTransport::Publisher::Publisher( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) : node_(node), object_odom_publisher_(object_odom_publisher), multi_object_odom_path_publisher_(multi_object_odom_path_publisher), @@ -274,19 +287,23 @@ DSDTransport::Publisher::Publisher( timestamp_(timestamp), timestamp_km1_(getPrevTimestamp(frame_timestamp_map, frame_id, timestamp)), // added to get (k-1)th timestamp object_odometries_(DSDTransport::constructObjectOdometries( - motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link)), + motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link, object_class_map)), object_paths_(DSDTransport::constructMultiObjectOdometryPaths( - motions, poses, timestamp, frame_timestamp_map, frame_id_link)) {} + motions, poses, timestamp, frame_timestamp_map, frame_id_link, true, object_class_map)), + object_class_map_(object_class_map){} + DSDTransport::Publisher DSDTransport::addObjectInfo( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, - const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) { + const FrameIdTimestampMap& frame_timestamp_map, + FrameId frame_id, + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) { return Publisher(node_, object_odom_publisher_, multi_object_odom_path_publisher_, tf_broadcaster_, motions, poses, frame_id_link, frame_timestamp_map, frame_id, - timestamp); + timestamp, object_class_map); } DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc index 670eb9216..e7099fa35 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc @@ -216,10 +216,11 @@ void FrontendDSDRos::tryPublishObjects( const auto& object_motions = object_motions_; const auto& object_poses = object_poses_; const auto& timestamp_map = this->shared_module_info.getTimestampMap(); + const auto& label_map = this->shared_module_info.getClassLabelMap(); DSDTransport::Publisher object_poses_publisher = dsd_transport_.addObjectInfo( object_motions, object_poses, params_.world_frame_id, timestamp_map, - frontend_output->frameId(), frontend_output->timestamp()); + frontend_output->frameId(), frontend_output->timestamp(), label_map); object_poses_publisher.publishObjectOdometry(); object_poses_publisher.publishObjectTransforms(); object_poses_publisher.publishObjectPaths(); diff --git a/dynosam_utils/src/install/.colcon_install_layout b/dynosam_utils/src/install/.colcon_install_layout new file mode 100644 index 000000000..3aad5336a --- /dev/null +++ b/dynosam_utils/src/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/dynosam_utils/src/install/COLCON_IGNORE b/dynosam_utils/src/install/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/install/_local_setup_util_ps1.py b/dynosam_utils/src/install/_local_setup_util_ps1.py new file mode 100644 index 000000000..3c6d9e877 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/_local_setup_util_sh.py b/dynosam_utils/src/install/_local_setup_util_sh.py new file mode 100644 index 000000000..f67eaa989 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/local_setup.bash b/dynosam_utils/src/install/local_setup.bash new file mode 100644 index 000000000..03f00256c --- /dev/null +++ b/dynosam_utils/src/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.ps1 b/dynosam_utils/src/install/local_setup.ps1 new file mode 100644 index 000000000..6f68c8ded --- /dev/null +++ b/dynosam_utils/src/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/dynosam_utils/src/install/local_setup.sh b/dynosam_utils/src/install/local_setup.sh new file mode 100644 index 000000000..e6ca4169e --- /dev/null +++ b/dynosam_utils/src/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/dev_ws/src/core/dynosam_utils/src/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.zsh b/dynosam_utils/src/install/local_setup.zsh new file mode 100644 index 000000000..b6487102f --- /dev/null +++ b/dynosam_utils/src/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.bash b/dynosam_utils/src/install/setup.bash new file mode 100644 index 000000000..396f8082a --- /dev/null +++ b/dynosam_utils/src/install/setup.bash @@ -0,0 +1,34 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/dynosam_utils/src/install/setup.ps1 b/dynosam_utils/src/install/setup.ps1 new file mode 100644 index 000000000..828baa9aa --- /dev/null +++ b/dynosam_utils/src/install/setup.ps1 @@ -0,0 +1,30 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/kilted\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/user/dev_ws/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/dynosam_utils/src/install/setup.sh b/dynosam_utils/src/install/setup.sh new file mode 100644 index 000000000..ad88868fb --- /dev/null +++ b/dynosam_utils/src/install/setup.sh @@ -0,0 +1,49 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/dev_ws/src/core/dynosam_utils/src/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.zsh b/dynosam_utils/src/install/setup.zsh new file mode 100644 index 000000000..fbb28ce08 --- /dev/null +++ b/dynosam_utils/src/install/setup.zsh @@ -0,0 +1,34 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/dynosam_utils/src/log/COLCON_IGNORE b/dynosam_utils/src/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log new file mode 100644 index 000000000..aac44b638 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000135] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log new file mode 100644 index 000000000..e731db663 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log @@ -0,0 +1,64 @@ +[0.220s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.220s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.273s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[0.326s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[0.327s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[0.479s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[0.480s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[0.496s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[0.507s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[0.508s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[0.518s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[0.519s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[0.521s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[0.523s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[0.525s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[0.526s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[0.527s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[0.529s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[0.530s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[0.531s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[0.532s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log new file mode 100644 index 000000000..d44325331 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000461] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log new file mode 100644 index 000000000..4dcaa6f21 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log @@ -0,0 +1,65 @@ +[0.726s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.726s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.882s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.882s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.884s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[1.029s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 10 installed packages in /home/user/dev_ws/install +[1.038s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[1.039s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[1.335s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[1.336s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.337s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.377s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.383s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[1.384s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.397s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[1.398s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[1.401s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[1.404s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[1.407s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[1.408s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[1.409s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[1.412s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[1.415s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[1.417s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[1.420s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/latest b/dynosam_utils/src/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/dynosam_utils/src/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/dynosam_utils/src/log/latest_build b/dynosam_utils/src/log/latest_build new file mode 120000 index 000000000..d799f4781 --- /dev/null +++ b/dynosam_utils/src/log/latest_build @@ -0,0 +1 @@ +build_2026-01-28_00-38-18 \ No newline at end of file diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py old mode 100644 new mode 100755 From 57038e0ecd7e7ba6f5efd1c22738cd1ad82ef0be Mon Sep 17 00:00:00 2001 From: Ethan Date: Wed, 17 Dec 2025 15:04:41 +1100 Subject: [PATCH 09/13] Added velocities to odometry message --- .../frontend/RGBDInstanceFrontendModule.hpp | 7 +++ .../frontend/VisionImuOutputPacket.hpp | 11 +++- .../frontend/vision/FeatureTracker.hpp | 2 +- .../frontend/RGBDInstanceFrontendModule.cc | 7 +++ dynosam/src/frontend/VisionImuOutputPacket.cc | 28 +++++++++ dynosam/src/frontend/vision/FeatureTracker.cc | 53 +++++++++++++++- .../include/dynosam_common/DynamicObjects.hpp | 6 +- dynosam_common/src/DynamicObjects.cc | 39 +++++++++++- .../include/dynosam_cv/ImageContainer.hpp | 5 ++ dynosam_cv/src/ImageContainer.cc | 15 +++++ .../dynosam_ros/displays/DisplaysCommon.hpp | 3 +- .../dynamic_slam_displays/DSDCommonRos.hpp | 8 ++- dynosam_ros/src/RosUtils.cc | 18 ++++++ dynosam_ros/src/displays/DisplaysCommon.cc | 12 +++- .../dynamic_slam_displays/DSDCommonRos.cc | 61 +++++++++++++++---- .../dynamic_slam_displays/FrontendDSDRos.cc | 7 ++- dynosam_utils/src/run_experiments_ecmr.py | 17 +++--- 17 files changed, 259 insertions(+), 40 deletions(-) diff --git a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp index 1b17db39a..3545f3584 100644 --- a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp @@ -126,6 +126,13 @@ class RGBDInstanceFrontendModule : public FrontendModule { //! Last keyframe Frame::Ptr frame_lkf_; + + // Previous packet + VisionImuPacket::Ptr previous_vision_imu_packet_; + + }; + + } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp index 563951a06..286b44583 100644 --- a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp +++ b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp @@ -80,7 +80,7 @@ class VisionImuPacket { }; /** - * @brief Object track information epresenting visual measurements for a + * @brief Object track information representing visual measurements for a * single object as well as frame-to-frame (and possibly other) motion/pose * information. * @@ -99,6 +99,8 @@ class VisionImuPacket { ImuFrontend::PimPtr pim() const; Camera::ConstPtr camera() const; PointCloudLabelRGB::Ptr denseLabelledCloud() const; + gtsam::Vector6 getBodyVelocity() const; + bool isCameraKeyFrame() const; bool isObjectKeyFrame(ObjectId object_id) const; @@ -107,6 +109,7 @@ class VisionImuPacket { const CameraTracks& cameraTracks() const; const gtsam::Pose3& cameraPose() const; + /** * @brief Returns the relative camera motion T_k_1_k, representing the motion * of the camera from k-1 to k in the camera local frame (at k-1) @@ -149,6 +152,7 @@ class VisionImuPacket { VisionImuPacket& groundTruthPacket( const GroundTruthInputPacket::Optional& gt); VisionImuPacket& debugImagery(const DebugImagery::Optional& dbg); + VisionImuPacket& updateBodyVelocity(const VisionImuPacket& prev_state); bool operator==(const VisionImuPacket& other) const { return frame_id_ == other.frame_id_ && timestamp_ == other.timestamp_; @@ -196,6 +200,11 @@ class VisionImuPacket { static void fillLandmarkMeasurements( StatusLandmarkVector& landmarks, const CameraMeasurementStatusVector& camera_measurements); + + + private: + std::optional stored_body_velocity; + }; } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp index 7040d271f..7391d93da 100644 --- a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp +++ b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp @@ -146,7 +146,7 @@ class FeatureTracker : public FeatureTrackerBase { private: // TODO: for now we loose the actual object detection result if inference was // run! - bool objectDetection( + std::optional objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container); diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index d67875711..dee609458 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -231,6 +231,13 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( fillOutputPacketWithTracks(vision_imu_packet, *frame, T_k_1_k, object_motions, object_poses); + // Update the body velocity according to the previous vision IMU packet + if (previous_vision_imu_packet_) { + vision_imu_packet->updateBodyVelocity(*previous_vision_imu_packet_); + } + + previous_vision_imu_packet_ = vision_imu_packet; + if (R_curr_ref) { imu_frontend_.resetIntegration(); } diff --git a/dynosam/src/frontend/VisionImuOutputPacket.cc b/dynosam/src/frontend/VisionImuOutputPacket.cc index e4c24fe02..5d1e79eca 100644 --- a/dynosam/src/frontend/VisionImuOutputPacket.cc +++ b/dynosam/src/frontend/VisionImuOutputPacket.cc @@ -40,6 +40,8 @@ ImuFrontend::PimPtr VisionImuPacket::pim() const { return pim_; } Camera::ConstPtr VisionImuPacket::camera() const { return camera_; } +gtsam::Vector6 VisionImuPacket::getBodyVelocity() const { return *stored_body_velocity; } + PointCloudLabelRGB::Ptr VisionImuPacket::denseLabelledCloud() const { return dense_labelled_cloud_; } @@ -213,6 +215,32 @@ void VisionImuPacket::fillLandmarkMeasurements( } } +// Adding velocity information into the VisionImuPacket + +// * Function to update the previous body velocity * +VisionImuPacket& VisionImuPacket::updateBodyVelocity(const VisionImuPacket& prev_state) { + + // Getting the pose at k-1 + const gtsam::Pose3& w_L_k_1 = prev_state.cameraPose(); + + // Getting the motion from k-1 to k + const gtsam::Pose3& w_k_1_H_k = camera_tracks_.T_k_1_k; + + stored_body_velocity = calculateBodyMotion( + w_k_1_H_k, + w_L_k_1, + prev_state.timestamp(), + timestamp_ + ); + + return *this; + +} + +// * Function to get the body velocity * + + + } // namespace dyno namespace nlohmann { diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index 4d48925e0..c4b2dbc84 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -47,6 +47,32 @@ namespace dyno { + // HELPER FUNCTION ADDED BY ETHAN +// map for classes to number IDs (so we can expand later) +static const std::map class_name_to_id{ + {"Undefined", ClassSegmentation::Undefined}, // 0 + {"Road", ClassSegmentation::Road}, // 1 + {"Rider", ClassSegmentation::Rider}, // 2 +}; + +int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { + + auto it = object_classes.find(obj_id); // find the object in the map + + if (it == object_classes.end()) { + return ClassSegmentation::Undefined; + } + + auto class_it = class_name_to_id.find(it->second); + if (class_it != class_name_to_id.end()) { + return class_it->second; + } + + return ClassSegmentation::Undefined; + +} + + FeatureTracker::FeatureTracker(const FrontendParams& params, Camera::Ptr camera, ImageDisplayQueue* display_queue) : FeatureTrackerBase(params.tracker_params, camera, display_queue), @@ -102,6 +128,7 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // result after the function call) ObjectDetectionEngine is used if // params_.prefer_provided_object_detection is false vision_tools::ObjectBoundaryMaskResult boundary_mask_result; + objectDetection(boundary_mask_result, input_images); if (!initial_computation_ && params_.use_propogate_mask) { @@ -1148,7 +1175,7 @@ void FeatureTracker::requiresSampling( } } -bool FeatureTracker::objectDetection( +std::optional FeatureTracker::objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container) { // from some experimental testing 10 pixles is a good boarder to add around @@ -1179,7 +1206,7 @@ bool FeatureTracker::objectDetection( vision_tools::computeObjectMaskBoundaryMask( boundary_mask_result, object_mask, scaled_boarder_thickness, kUseAsFeatureDetectionMask); - return false; + return std::nullopt; } else { LOG(FATAL) << "Params specify prefer provided object mask but input " "is missing!"; @@ -1205,7 +1232,7 @@ bool FeatureTracker::objectDetection( // update or insert image container with object mask image_container.replace(ImageContainer::kObjectMask, object_mask); - return true; + return detection_result; } } @@ -1220,6 +1247,18 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // note reference cv::Mat& current_mask = image_container.objectMotionMask(); + cv::Mat& current_class_mask = image_container.objectClassMask(); + + // building map of previous classes + std::map object_classes; + for (const auto& detection : previous_frame->object_detection_.detections) { + if (detection.isValid()) { + object_classes[detection.obj_id] = detection.class_name; + } + else { + object_classes[detection.obj_id] = "Unknown"; + } + } ObjectIds instance_labels; for (const Feature::Ptr& dynamic_feature : @@ -1348,6 +1387,13 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { current_mask.at(functional_keypoint::v(predicted_kp), functional_keypoint::u(predicted_kp)) = instance_labels[i]; + + const ObjectId obj_id = instance_labels[i]; + int class_id = objectIdToClassId(obj_id, object_classes); + + current_class_mask.at(functional_keypoint::v(predicted_kp), + functional_keypoint::u(predicted_kp)) = + class_id; // current_rgb // updated_mask_points++; } @@ -1358,4 +1404,5 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { } } + } // namespace dyno diff --git a/dynosam_common/include/dynosam_common/DynamicObjects.hpp b/dynosam_common/include/dynosam_common/DynamicObjects.hpp index ef74681d6..82d33a61f 100644 --- a/dynosam_common/include/dynosam_common/DynamicObjects.hpp +++ b/dynosam_common/include/dynosam_common/DynamicObjects.hpp @@ -122,8 +122,10 @@ struct ObjectDetectionResult { * @param w_L_k_1 const gtsam::Pose3& * @return gtsam::Vector3 */ -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1); +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k); enum PropogateType { InitGT, diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index b136c7b2d..1b67c1e33 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -73,15 +73,48 @@ std::ostream& operator<<(std::ostream& os, return os; } -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1) { +// want this to also output angular velocity too +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k) { const gtsam::Point3& t_motion = w_k_1_H_k.translation(); const gtsam::Rot3& R_motion = w_k_1_H_k.rotation(); const gtsam::Point3& t_pose = w_L_k_1.translation(); + const gtsam::Rot3& R_pose = w_L_k_1.rotation(); static const gtsam::Rot3 I = gtsam::Rot3::Identity(); - return t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose; + Timestamp dt = 0.04; + + gtsam::Vector3 trans_vel = (t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose) / dt; + +// ===== NOW CALCULATING THE ANGULAR VELOCITY ===== + + // Finding the relative rotation over the timestep + gtsam::Rot3 R_pose_transpose = R_pose.inverse(); + gtsam::Rot3 R_new = R_motion * R_pose; + gtsam::Rot3 R_rel = R_pose_transpose * R_new; + + // Calculating the trace of the relative rotation and the angle of rotation + double trace = R_rel.matrix().trace(); + double angle = std::acos((trace - 1) / 2); + + gtsam::Matrix3 R_rel_m = R_rel.matrix(); + + // Finding the unit axis of rotation + double ux = (1 / (2 * std::sin(angle))) * (R_rel_m(2,1) - R_rel_m(1,2)); + double uy = (1 / (2 * std::sin(angle))) * (R_rel_m(0,2) - R_rel_m(2,0)); + double uz = (1 / (2 * std::sin(angle))) * (R_rel_m(1,0) - R_rel_m(0,1)); + const gtsam::Vector3 axis(ux, uy, uz); + + const gtsam::Vector3 angular_vel = (angle / dt) * axis; + + gtsam::Vector6 body_velocity; + body_velocity.head<3>() = trans_vel; + body_velocity.tail<3>() = angular_vel; + + return body_velocity; } void propogateObjectPoses(ObjectPoseMap& object_poses, diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index efdda9995..32d7959b8 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,6 +230,7 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; + static constexpr char kClassMask[] = "classmask" static constexpr char kRightRgb[] = "rightrgb"; public: @@ -306,24 +307,28 @@ class ImageContainer { inline bool hasDepth() const { return exists(kDepth); } inline bool hasOpticalFlow() const { return exists(kOPticalFlow); } inline bool hasObjectMask() const { return exists(kObjectMask); } + inline bool hasClassMask() const { return exists(kClassMask); } inline bool hasRightRgb() const { return exists(kRightRgb); } const ImageWrapper& rgb() const; const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; + const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); + ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); ImageContainer& depth(const cv::Mat& image); ImageContainer& opticalFlow(const cv::Mat& image); ImageContainer& objectMotionMask(const cv::Mat& image); + ImageContainer& objectClassMask(const cv::Mat& image); ImageContainer& rightRgb(const cv::Mat& image); Timestamp timestamp() const { return timestamp_; } diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index 2dca31a91..31c072bae 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -62,6 +62,12 @@ const ImageWrapper& ImageContainer::objectMotionMask() const { return this->at(kObjectMask); } + +const ImageWrapper& ImageContainer::objectClassMask() + const { + return this->at(kClassMask); + } + const ImageWrapper& ImageContainer::rightRgb() const { return this->at(kRightRgb); } @@ -78,6 +84,11 @@ ImageWrapper& ImageContainer::opticalFlow() { ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } + +ImageWrapper& ImageContainer::objectClassMask() { + return this->at(kClassMask); +} + ImageWrapper& ImageContainer::rightRgb() { return this->at(kRightRgb); } @@ -98,6 +109,10 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { return this->add(kObjectMask, image); } +ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { + return this->add(kClassMask, image); +} + ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { return this->add(kRightRgb, image); } diff --git a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp index 78f0f946e..daab0c1ce 100644 --- a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp @@ -68,7 +68,8 @@ struct DisplayCommon { static void publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id); + const std::string& child_frame_id, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); static void publishOdometryPath(PathPub::SharedPtr pub, const gtsam::Pose3Vector& poses, Timestamp latest_timestamp, diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp index d0e38aa15..a203455c3 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp @@ -83,12 +83,12 @@ class DSDTransport { // to get velocity... static ObjectOdometry constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link, const std::string& child_frame_id_link); static ObjectOdometryMap constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link); static MultiObjectOdometryPath constructMultiObjectOdometryPaths( @@ -146,6 +146,7 @@ class DSDTransport { std::string frame_id_link_; FrameId frame_id_; Timestamp timestamp_; + Timestamp timestamp_km1_; //! Object odometries for this frame ObjectOdometryMap object_odometries_; @@ -231,7 +232,8 @@ class DSDRos { DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node); void publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf); + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); void publishVisualOdometryPath(const gtsam::Pose3Vector& poses, Timestamp latest_timestamp); diff --git a/dynosam_ros/src/RosUtils.cc b/dynosam_ros/src/RosUtils.cc index 73d197092..6be26d1e6 100644 --- a/dynosam_ros/src/RosUtils.cc +++ b/dynosam_ros/src/RosUtils.cc @@ -158,6 +158,24 @@ bool dyno::convert(const gtsam::Pose3& pose, return true; } +// NEW! Template to add twist to the message +template <> +bool dyno::convert(const gtsam::Vector6& vel, + geometry_msgs::msg::Twist& twist) +{ + // linear velocity components + twist.linear.x = vel(0); + twist.linear.y = vel(1); + twist.linear.z = vel(2); + + // angular velocity components + twist.angular.x = vel(3); + twist.angular.y = vel(4); + twist.angular.z = vel(5); + + return true; +} + template <> bool dyno::convert(const gtsam::Pose3& pose, geometry_msgs::msg::TransformStamped& transform) { diff --git a/dynosam_ros/src/displays/DisplaysCommon.cc b/dynosam_ros/src/displays/DisplaysCommon.cc index 548523ef0..0cd0f9a2d 100644 --- a/dynosam_ros/src/displays/DisplaysCommon.cc +++ b/dynosam_ros/src/displays/DisplaysCommon.cc @@ -44,15 +44,23 @@ CloudPerObject DisplayCommon::publishPointCloud( return clouds_per_obj; } +// ================================================================================================================ +// TODO: look here edit this function to include velocities +// ================================================================================================================ + void DisplayCommon::publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id) { + const std::string& child_frame_id, + const gtsam::Vector6& velocity + ) { nav_msgs::msg::Odometry odom_msg; utils::convertWithHeader(T_world_camera, odom_msg, timestamp, frame_id, child_frame_id); - pub->publish(odom_msg); + + dyno::convert(velocity, odom_msg.twist.twist); + pub->publish(odom_msg); } void DisplayCommon::publishOdometryPath(PathPub::SharedPtr pub, diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc index 4624adf4b..5a180ee9a 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc @@ -25,24 +25,29 @@ std::string DSDTransport::constructObjectFrameLink(ObjectId object_id) { return "object_" + std::to_string(object_id) + "_link"; } +// ================================================================================================================ +// TODO: Fill in the velocity and check calculateBodyMotion is correct (only linear, add angular component too) +// ================================================================================================================ ObjectOdometry DSDTransport::constructObjectOdometry( - const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, + const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link, const std::string& child_frame_id_link) { ObjectOdometry object_odom; - // technically this shoudl be k-1 - gtsam::Point3 body_velocity = calculateBodyMotion(e_H_k_world, pose_k); + // technically this should be k-1 + gtsam::Vector6 body_velocity = calculateBodyMotion(e_H_k_world, pose_km1, timestamp_km1, timestamp_k); nav_msgs::msg::Odometry odom_msg; - utils::convertWithHeader(pose_k, odom_msg, timestamp_k, frame_id_link, + utils::convertWithHeader(pose_km1, odom_msg, timestamp_k, frame_id_link, child_frame_id_link); object_odom.odom = odom_msg; // TODO: can check if correct representation? dyno::convert(e_H_k_world, object_odom.h_w_km1_k.pose); - // NO velocity!! + + dyno::convert(body_velocity, object_odom.odom.twist.twist); // Add velocity to message + object_odom.object_id = object_id; object_odom.sequence = frame_id_k; return object_odom; @@ -50,7 +55,7 @@ ObjectOdometry DSDTransport::constructObjectOdometry( ObjectOdometryMap DSDTransport::constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, const std::string& frame_id_link) { // need to get poses for k-1 // TODO: no way to ensure that the motions are for frame k @@ -79,8 +84,8 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( object_odom_map.insert2( child_frame_id_link, constructObjectOdometry(e_H_k_world, pose_k, object_id, frame_id_k, - timestamp_k, frame_id_link, - child_frame_id_link)); + timestamp_k, timestamp_km1, + frame_id_link, child_frame_id_link)); } return object_odom_map; @@ -139,9 +144,19 @@ MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( // RIGHT NOW MOTION IDENTITY // timestamp is wrong + + // Adding the (k-1)th timestamp + Timestamp timestamp_km1; + if (previous_frame_id >= 0) { + timestamp_km1 = frame_timestamp_map.at(previous_frame_id); + } + else { + timestamp_km1 = timestamp; + } + gtsam::Pose3 motion; const ObjectOdometry object_odometry = constructObjectOdometry( - object_motion, object_pose, object_id, frame_id, timestamp, + object_motion, object_pose, object_id, frame_id, timestamp, timestamp_km1, frame_id_link, child_frame_id_link); if (!segmented_paths.exists(path_segment)) { @@ -224,6 +239,23 @@ void DSDTransport::Publisher::publishObjectPaths() { multi_object_odom_path_publisher_->publish(object_paths_); } +// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTEP ============================== +Timestamp getPrevTimestamp(const FrameIdTimestampMap& map, FrameId id, Timestamp current_timestamp) { + + if (id < 0) return current_timestamp; + + FrameId prev_id = id - 1; + + auto it = map.find(prev_id); + if (it != map.end()) { + return it->second; + } + else { + return current_timestamp; + } +} +// ============================================================================================================ + DSDTransport::Publisher::Publisher( rclcpp::Node::SharedPtr node, ObjectOdometryPub::SharedPtr object_odom_publisher, @@ -240,8 +272,9 @@ DSDTransport::Publisher::Publisher( frame_id_link_(frame_id_link), frame_id_(frame_id), timestamp_(timestamp), + timestamp_km1_(getPrevTimestamp(frame_timestamp_map, frame_id, timestamp)), // added to get (k-1)th timestamp object_odometries_(DSDTransport::constructObjectOdometries( - motions, poses, frame_id, timestamp, frame_id_link)), + motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link)), object_paths_(DSDTransport::constructMultiObjectOdometryPaths( motions, poses, timestamp, frame_timestamp_map, frame_id_link)) {} @@ -271,10 +304,12 @@ DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) } void DSDRos::publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf) { + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity) { DisplayCommon::publishOdometry(vo_publisher_, T_world_camera, timestamp, params_.world_frame_id, - params_.camera_frame_id); + params_.camera_frame_id, + velocity); if (publish_tf) { geometry_msgs::msg::TransformStamped t; diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc index fe50cebfe..670eb9216 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc @@ -173,9 +173,12 @@ void FrontendDSDRos::tryPublishGroundTruth( void FrontendDSDRos::tryPublishVisualOdometry( const VisionImuPacket::ConstPtr& frontend_output) { // publish vo - constexpr static bool kPublishOdomAsTf = true; + constexpr static bool kPublishOdomAsTf = true; + this->publishVisualOdometry(frontend_output->cameraPose(), - frontend_output->timestamp(), kPublishOdomAsTf); + frontend_output->timestamp(), + kPublishOdomAsTf, + frontend_output->getBodyVelocity()); // relies on correct accumulation of internal objects this->publishVisualOdometryPath(camera_poses_, frontend_output->timestamp()); diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py index 8af56e3d5..906b4526a 100644 --- a/dynosam_utils/src/run_experiments_ecmr.py +++ b/dynosam_utils/src/run_experiments_ecmr.py @@ -33,7 +33,6 @@ def run_sequnce(path, name, data_loader_num, backend_type, *args, **kwargs): parsed_args["launch_file"] = "dyno_sam_launch.py" - if run_as_frontend: additional_args.extend([ "--use_backend=0", @@ -155,13 +154,13 @@ def run_viodes(): # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/mid", "viode_city_day_mid", viode, "--v=100") # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","viode_city_day_high", viode, "--ending_frame=1110") - run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode1", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") # # zero_elements_ratio # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/mid", "viode_city_night_mid", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/high", "viode_city_night_high", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/mid", "parking_lot_night_mid", viode) - # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/high", "parking_lot_night_high", viode) + run_ecmr_experiment_sequences("/root/data/viode_parking_lot/high", "parking_lot_night_high", viode) def run_omd(): run_ecmr_experiment_sequences("/root/data/vdo_slam/omd/omd/swinging_4_unconstrained_stereo/","test", omd_dataset, "--ending_frame=300") @@ -173,7 +172,7 @@ def run_tartan_air(): run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing05", "test_tartan", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing06", "tas_rc6", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing07", "tas_rc7", tartan_air, "--starting_frame=5", "--ending_frame=65") - # run_analysis("tas_rc7") + # run_analysis("tas_rc7") # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing01", "tas_s1", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing02", "tas_s2", tartan_air) @@ -192,8 +191,8 @@ def run_kitti(): # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0005/", "kitti_0005_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0006/", "kitti_0006_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0018/", "kitti_0018_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0004/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + run_ecmr_experiment_sequences("/root/data/kitti_0001/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") def run_aria(): @@ -207,10 +206,10 @@ def run_aria(): # "--ending_frame=150" # ) - # run_viodes() + run_viodes() # run_tartan_air() # run_cluster() - run_omd() + # run_omd() # run_aria() # run_kitti() # run_analysis("kitti_0001_static_only") @@ -499,4 +498,4 @@ def run_aria(): # "kitti_0003", # backend_type=object_centric_batch # ) - # run_analysis("kitti_0003") + # run_analysis("kitti_0003") \ No newline at end of file From 9856793e26b30669a51441544cb3a817c8dc6af0 Mon Sep 17 00:00:00 2001 From: Ethan Date: Wed, 17 Dec 2025 16:28:43 +1100 Subject: [PATCH 10/13] Smaller fixes --- dynosam/src/frontend/vision/FeatureTracker.cc | 22 +++++++++---------- .../include/dynosam_cv/ImageContainer.hpp | 6 ++--- dynosam_cv/src/ImageContainer.cc | 10 ++++----- dynosam_utils/src/run_experiments_ecmr.py | 4 ++-- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index c4b2dbc84..a0a4da59f 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -48,19 +48,19 @@ namespace dyno { // HELPER FUNCTION ADDED BY ETHAN -// map for classes to number IDs (so we can expand later) -static const std::map class_name_to_id{ - {"Undefined", ClassSegmentation::Undefined}, // 0 - {"Road", ClassSegmentation::Road}, // 1 - {"Rider", ClassSegmentation::Rider}, // 2 +// TODO: make this map for the class labels +static const std::map class_name_to_id{ + {"Undefined", 0}, // 0 + {"Road", 1}, // 1 + {"Rider", 2}, // 2 }; int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { auto it = object_classes.find(obj_id); // find the object in the map - if (it == object_classes.end()) { - return ClassSegmentation::Undefined; + if (it == object_classes.end() || it->second == "Unknown") { + return 0; } auto class_it = class_name_to_id.find(it->second); @@ -68,7 +68,7 @@ int objectIdToClassId(ObjectId obj_id, const std::map& ob return class_it->second; } - return ClassSegmentation::Undefined; + return 1; } @@ -1251,12 +1251,12 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // building map of previous classes std::map object_classes; - for (const auto& detection : previous_frame->object_detection_.detections) { + for (const auto& [obj_id, detection] : previous_frame_->object_observations_) { if (detection.isValid()) { - object_classes[detection.obj_id] = detection.class_name; + object_classes[obj_id] = detection.class_name; } else { - object_classes[detection.obj_id] = "Unknown"; + object_classes[obj_id] = "Unknown"; } } diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index 32d7959b8..17034a064 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,7 +230,7 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; - static constexpr char kClassMask[] = "classmask" + static constexpr char kClassMask[] = "classmask"; static constexpr char kRightRgb[] = "rightrgb"; public: @@ -314,14 +314,14 @@ class ImageContainer { const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; - const ImageWrapper& objectClassMask() const; + const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); - ImageWrapper& objectClassMask(); + ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index 31c072bae..efefe2e3f 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -63,9 +63,9 @@ const ImageWrapper& ImageContainer::objectMotionMask() return this->at(kObjectMask); } -const ImageWrapper& ImageContainer::objectClassMask() +const ImageWrapper& ImageContainer::objectClassMask() const { - return this->at(kClassMask); + return this->at(kClassMask); } const ImageWrapper& ImageContainer::rightRgb() const { @@ -85,8 +85,8 @@ ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } -ImageWrapper& ImageContainer::objectClassMask() { - return this->at(kClassMask); +ImageWrapper& ImageContainer::objectClassMask() { + return this->at(kClassMask); } ImageWrapper& ImageContainer::rightRgb() { @@ -110,7 +110,7 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { } ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { - return this->add(kClassMask, image); + return this->add(kClassMask, image); } ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py index 906b4526a..e115f1b5e 100644 --- a/dynosam_utils/src/run_experiments_ecmr.py +++ b/dynosam_utils/src/run_experiments_ecmr.py @@ -206,12 +206,12 @@ def run_aria(): # "--ending_frame=150" # ) - run_viodes() + # run_viodes() # run_tartan_air() # run_cluster() # run_omd() # run_aria() - # run_kitti() + run_kitti() # run_analysis("kitti_0001_static_only") # run_analysis("kitti_0002_static_only") # run_analysis("kitti_0003_static_only") From c718d83c857d6bce3e66d3bc0c04a186a9cef7ec Mon Sep 17 00:00:00 2001 From: Ethan Date: Thu, 29 Jan 2026 14:46:22 +1100 Subject: [PATCH 11/13] Added semantic label passthrough --- .../dynosam/frontend/FrontendModule.hpp | 4 + .../frontend/vision/FeatureTracker.hpp | 4 +- dynosam/src/frontend/FrontendModule.cc | 14 + .../frontend/RGBDInstanceFrontendModule.cc | 4 + dynosam/src/frontend/vision/FeatureTracker.cc | 90 ++-- .../dynosam_common/SharedModuleInfo.hpp | 3 + .../include/dynosam_common/Types.hpp | 3 + dynosam_common/src/DynamicObjects.cc | 23 +- dynosam_common/src/SharedModuleInfo.cc | 18 + .../include/dynosam_cv/ImageContainer.hpp | 5 - dynosam_cv/src/ImageContainer.cc | 12 - .../dynamic_slam_displays/DSDCommonRos.hpp | 17 +- .../dynamic_slam_displays/DSDCommonRos.cc | 41 +- .../dynamic_slam_displays/FrontendDSDRos.cc | 3 +- .../src/install/.colcon_install_layout | 1 + dynosam_utils/src/install/COLCON_IGNORE | 0 .../src/install/_local_setup_util_ps1.py | 407 ++++++++++++++++++ .../src/install/_local_setup_util_sh.py | 407 ++++++++++++++++++ dynosam_utils/src/install/local_setup.bash | 121 ++++++ dynosam_utils/src/install/local_setup.ps1 | 55 +++ dynosam_utils/src/install/local_setup.sh | 137 ++++++ dynosam_utils/src/install/local_setup.zsh | 134 ++++++ dynosam_utils/src/install/setup.bash | 34 ++ dynosam_utils/src/install/setup.ps1 | 30 ++ dynosam_utils/src/install/setup.sh | 49 +++ dynosam_utils/src/install/setup.zsh | 34 ++ dynosam_utils/src/log/COLCON_IGNORE | 0 .../log/build_2026-01-27_04-03-52/events.log | 2 + .../build_2026-01-27_04-03-52/logger_all.log | 64 +++ .../log/build_2026-01-28_00-38-18/events.log | 2 + .../build_2026-01-28_00-38-18/logger_all.log | 65 +++ dynosam_utils/src/log/latest | 1 + dynosam_utils/src/log/latest_build | 1 + dynosam_utils/src/run_experiments_ecmr.py | 0 34 files changed, 1690 insertions(+), 95 deletions(-) create mode 100644 dynosam_utils/src/install/.colcon_install_layout create mode 100644 dynosam_utils/src/install/COLCON_IGNORE create mode 100644 dynosam_utils/src/install/_local_setup_util_ps1.py create mode 100644 dynosam_utils/src/install/_local_setup_util_sh.py create mode 100644 dynosam_utils/src/install/local_setup.bash create mode 100644 dynosam_utils/src/install/local_setup.ps1 create mode 100644 dynosam_utils/src/install/local_setup.sh create mode 100644 dynosam_utils/src/install/local_setup.zsh create mode 100644 dynosam_utils/src/install/setup.bash create mode 100644 dynosam_utils/src/install/setup.ps1 create mode 100644 dynosam_utils/src/install/setup.sh create mode 100644 dynosam_utils/src/install/setup.zsh create mode 100644 dynosam_utils/src/log/COLCON_IGNORE create mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log create mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log create mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log create mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log create mode 120000 dynosam_utils/src/log/latest create mode 120000 dynosam_utils/src/log/latest_build mode change 100644 => 100755 dynosam_utils/src/run_experiments_ecmr.py diff --git a/dynosam/include/dynosam/frontend/FrontendModule.hpp b/dynosam/include/dynosam/frontend/FrontendModule.hpp index 018588e51..1cbc140cc 100644 --- a/dynosam/include/dynosam/frontend/FrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/FrontendModule.hpp @@ -39,6 +39,8 @@ #include "dynosam_common/ModuleBase.hpp" #include "dynosam_common/SharedModuleInfo.hpp" #include "dynosam_common/Types.hpp" +#include "dynosam/frontend/vision/FeatureTracker.hpp" + // #include "dynosam_common" @@ -84,6 +86,8 @@ class FrontendModule virtual void onBackendUpdateCallback(const FrameId /*frame_id*/, const Timestamp /*timestamp*/) {} + void FrameToClassMap(const Frame::Ptr& frame) const; + protected: /** * @brief Defines the result of checking the image container which is a done diff --git a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp index 7391d93da..7a8ee78b7 100644 --- a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp +++ b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp @@ -36,10 +36,12 @@ #include "dynosam/frontend/FrontendParams.hpp" #include "dynosam/frontend/vision/FeatureTrackerBase.hpp" #include "dynosam/frontend/vision/Frame.hpp" +#include "dynosam/frontend/FrontendModule.hpp" #include "dynosam/frontend/vision/StaticFeatureTracker.hpp" #include "dynosam/visualizer/Visualizer-Definitions.hpp" #include "dynosam_cv/Camera.hpp" #include "dynosam_cv/Feature.hpp" +#include "dynosam_common/SharedModuleInfo.hpp" // #include "dynosam_common/DynamicObjects.hpp" #include "dynosam_nn/ObjectDetector.hpp" @@ -146,7 +148,7 @@ class FeatureTracker : public FeatureTrackerBase { private: // TODO: for now we loose the actual object detection result if inference was // run! - std::optional objectDetection( + ObjectDetectionResult objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container); diff --git a/dynosam/src/frontend/FrontendModule.cc b/dynosam/src/frontend/FrontendModule.cc index e084817d3..0bfaf3b0c 100644 --- a/dynosam/src/frontend/FrontendModule.cc +++ b/dynosam/src/frontend/FrontendModule.cc @@ -66,4 +66,18 @@ void FrontendModule::validateInput( result.valid_, *input->image_container_, result.requirement_); } +void FrontendModule::FrameToClassMap(const Frame::Ptr& frame) const { + const auto& object_observations = frame->getObjectObservations(); + + for (const auto& [object_id, detection] : object_observations) { + if (!detection.class_name.empty()) { + shared_module_info.updateClassLabelMapping(object_id, detection.class_name); + std::cout << "UPDATING CLASS LABEL MAPPING " << object_id << " IS " << detection.class_name << std::endl; + } + else { + std::cout << "CANNOT UPDATE CLASS LABEL MAPPING; IT WAS EMPTY!" << std::endl; + } + } +} + } // namespace dyno diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index dee609458..74952703a 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -116,6 +116,9 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::boostrapSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container); + FrontendModule::FrameToClassMap(frame); + + CHECK(frame->updateDepths()); return {State::Nominal, nullptr}; @@ -148,6 +151,7 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container, R_curr_ref); + FrontendModule::FrameToClassMap(frame); Frame::Ptr previous_frame = tracker_->getPreviousFrame(); CHECK(previous_frame); diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index a0a4da59f..f2cfb590a 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -47,32 +47,6 @@ namespace dyno { - // HELPER FUNCTION ADDED BY ETHAN -// TODO: make this map for the class labels -static const std::map class_name_to_id{ - {"Undefined", 0}, // 0 - {"Road", 1}, // 1 - {"Rider", 2}, // 2 -}; - -int objectIdToClassId(ObjectId obj_id, const std::map& object_classes) { - - auto it = object_classes.find(obj_id); // find the object in the map - - if (it == object_classes.end() || it->second == "Unknown") { - return 0; - } - - auto class_it = class_name_to_id.find(it->second); - if (class_it != class_name_to_id.end()) { - return class_it->second; - } - - return 1; - -} - - FeatureTracker::FeatureTracker(const FrontendParams& params, Camera::Ptr camera, ImageDisplayQueue* display_queue) : FeatureTrackerBase(params.tracker_params, camera, display_queue), @@ -129,7 +103,12 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // params_.prefer_provided_object_detection is false vision_tools::ObjectBoundaryMaskResult boundary_mask_result; - objectDetection(boundary_mask_result, input_images); + ObjectDetectionResult observations = objectDetection(boundary_mask_result, input_images); + + for (const auto& det : observations.detections) { + std::cout << "Detected object_id=" << det.object_id + << " class_name=" << det.class_name << std::endl; + } if (!initial_computation_ && params_.use_propogate_mask) { utils::ChronoTimingStats timer("propogate_mask"); @@ -182,18 +161,10 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // TODO: SingleDetectionResult really does not need the tracklet ids they // are never actually used!! this prevents the frame from needing to do the // same calculations we've alrady done - std::map object_observations; - for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { - ObjectId object_id = boundary_mask_result.objects_detected.at(i); - const cv::Rect& bb_detection = - boundary_mask_result.object_bounding_boxes.at(i); - - SingleDetectionResult observation; - observation.object_id = object_id; - // observation.object_features = dynamic_features.getByObject(object_id); - observation.bounding_box = bb_detection; - object_observations[object_id] = observation; + std::map object_observations; + for (const auto& detection : observations.detections) { + object_observations[detection.object_id] = detection; } utils::ChronoTimingStats f_timer("tracking_timer.frame_construction"); @@ -215,6 +186,8 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, previous_frame_ = new_frame; boarder_detection_mask_ = boundary_mask_result.boundary_mask; + // FrontendModule::FrameToClassMap(new_frame); + return new_frame; } @@ -1175,7 +1148,7 @@ void FeatureTracker::requiresSampling( } } -std::optional FeatureTracker::objectDetection( +ObjectDetectionResult FeatureTracker::objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container) { // from some experimental testing 10 pixles is a good boarder to add around @@ -1206,7 +1179,25 @@ std::optional FeatureTracker::objectDetection( vision_tools::computeObjectMaskBoundaryMask( boundary_mask_result, object_mask, scaled_boarder_thickness, kUseAsFeatureDetectionMask); - return std::nullopt; + + ObjectDetectionResult detection_result; + detection_result.input_image = image_container.rgb(); + detection_result.labelled_mask = object_mask; + + for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { + ObjectId object_id = boundary_mask_result.objects_detected.at(i); + const cv::Rect& bb_detection = + boundary_mask_result.object_bounding_boxes.at(i); + + SingleDetectionResult observation; + observation.object_id = object_id; + // observation.object_features = dynamic_features.getByObject(object_id); + observation.bounding_box = bb_detection; + observation.class_name = "goober"; + detection_result.detections.push_back(observation); + } + + return detection_result; } else { LOG(FATAL) << "Params specify prefer provided object mask but input " "is missing!"; @@ -1247,18 +1238,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // note reference cv::Mat& current_mask = image_container.objectMotionMask(); - cv::Mat& current_class_mask = image_container.objectClassMask(); - - // building map of previous classes - std::map object_classes; - for (const auto& [obj_id, detection] : previous_frame_->object_observations_) { - if (detection.isValid()) { - object_classes[obj_id] = detection.class_name; - } - else { - object_classes[obj_id] = "Unknown"; - } - } + ObjectIds instance_labels; for (const Feature::Ptr& dynamic_feature : @@ -1387,13 +1367,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { current_mask.at(functional_keypoint::v(predicted_kp), functional_keypoint::u(predicted_kp)) = instance_labels[i]; - - const ObjectId obj_id = instance_labels[i]; - int class_id = objectIdToClassId(obj_id, object_classes); - current_class_mask.at(functional_keypoint::v(predicted_kp), - functional_keypoint::u(predicted_kp)) = - class_id; // current_rgb // updated_mask_points++; } diff --git a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp index f88d28c2d..a7648b025 100644 --- a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp +++ b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp @@ -43,6 +43,7 @@ class SharedModuleInfo { std::optional getGroundTruthPackets() const; const FrameIdTimestampMap& getTimestampMap() const; + const ObjectIdClassMap& getClassLabelMap() const; bool getTimestamp(FrameId frame_id, Timestamp& timestamp) const; @@ -50,6 +51,7 @@ class SharedModuleInfo { FrameId frame_id, const GroundTruthInputPacket& ground_truth_packet); SharedModuleInfo& updateTimestampMapping(FrameId frame_id, Timestamp timestamp); + SharedModuleInfo& updateClassLabelMapping(ObjectId object_id, std::string class_label); private: static std::unique_ptr instance_; @@ -58,6 +60,7 @@ class SharedModuleInfo { mutable std::mutex mutex_; GroundTruthPacketMap gt_packet_map_; FrameIdTimestampMap frame_id_to_timestamp_map_; + ObjectIdClassMap object_id_to_class_map_; }; struct SharedModuleInterface { diff --git a/dynosam_common/include/dynosam_common/Types.hpp b/dynosam_common/include/dynosam_common/Types.hpp index b904202e8..677c54640 100644 --- a/dynosam_common/include/dynosam_common/Types.hpp +++ b/dynosam_common/include/dynosam_common/Types.hpp @@ -111,6 +111,9 @@ using MotionMap = /// @brief Map of FrameIds (k) to Timestamps using FrameIdTimestampMap = gtsam::FastMap; +/// @brief Map of Object Ids to CLass Labels +using ObjectIdClassMap = gtsam::FastMap; + // T is expected to have (at least) bitwise | (OR) support template class Flags { diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index 1b67c1e33..148cbd50c 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -73,6 +73,8 @@ std::ostream& operator<<(std::ostream& os, return os; } + +// OLD BODY MOTION CALCULATION IMPLEMENTATION // want this to also output angular velocity too gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, const gtsam::Pose3& w_L_k_1, @@ -85,7 +87,10 @@ gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, static const gtsam::Rot3 I = gtsam::Rot3::Identity(); - Timestamp dt = 0.04; + double dt = timestamp_k - timestamp_km1; + if (dt <= 1e-6) { + dt = 0.1; // fallback to nominal frame interval + } gtsam::Vector3 trans_vel = (t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose) / dt; @@ -117,6 +122,22 @@ gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, return body_velocity; } +// gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, +// const gtsam::Pose3& w_L_k_1, +// Timestamp timestamp_km1, +// Timestamp timestamp_k) { + +// // Finding relative pose +// Timestamp dt = 0.04; // Estimated time between frames in kitti datasets + +// gtsam::Pose3 T_rel = w_L_k_1.inverse() * (w_k_1_H_k * w_L_k_1); // if pose is given in world frame +// // gtsam::Pose3 T_rel = w_k_1_H_k; // if pose is already in body frame + +// gtsam::Vector6 body_velocity = gtsam::Pose3::Logmap(T_rel) / dt; + +// return body_velocity; +// } + void propogateObjectPoses(ObjectPoseMap& object_poses, const MotionEstimateMap& object_motions_k, const gtsam::Point3Vector& object_centroids_k_1, diff --git a/dynosam_common/src/SharedModuleInfo.cc b/dynosam_common/src/SharedModuleInfo.cc index 076b86213..c6c966a0e 100644 --- a/dynosam_common/src/SharedModuleInfo.cc +++ b/dynosam_common/src/SharedModuleInfo.cc @@ -57,6 +57,11 @@ std::optional SharedModuleInfo::getGroundTruthPackets() return gt_packet_map_; } +const ObjectIdClassMap& SharedModuleInfo::getClassLabelMap() const { + const std::lock_guard lock(mutex_); + return object_id_to_class_map_; +} + bool SharedModuleInfo::getTimestamp(FrameId frame_id, Timestamp& timestamp) const { const FrameIdTimestampMap& timestamp_map = getTimestampMap(); @@ -88,4 +93,17 @@ SharedModuleInfo& SharedModuleInfo::updateTimestampMapping( return *this; } +SharedModuleInfo& SharedModuleInfo::updateClassLabelMapping( + ObjectId object_id, std::string class_label) { + + const std::lock_guard lock(mutex_); + + if (object_id_to_class_map_.exists(object_id)) { + CHECK_EQ(object_id_to_class_map_.at(object_id), class_label); + } else { + object_id_to_class_map_.insert2(object_id, class_label); + } + return *this; +} + } // namespace dyno diff --git a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp index 17034a064..efdda9995 100644 --- a/dynosam_cv/include/dynosam_cv/ImageContainer.hpp +++ b/dynosam_cv/include/dynosam_cv/ImageContainer.hpp @@ -230,7 +230,6 @@ class ImageContainer { static constexpr char kOPticalFlow[] = "opticalflow"; static constexpr char kDepth[] = "depth"; static constexpr char kObjectMask[] = "objectmask"; - static constexpr char kClassMask[] = "classmask"; static constexpr char kRightRgb[] = "rightrgb"; public: @@ -307,28 +306,24 @@ class ImageContainer { inline bool hasDepth() const { return exists(kDepth); } inline bool hasOpticalFlow() const { return exists(kOPticalFlow); } inline bool hasObjectMask() const { return exists(kObjectMask); } - inline bool hasClassMask() const { return exists(kClassMask); } inline bool hasRightRgb() const { return exists(kRightRgb); } const ImageWrapper& rgb() const; const ImageWrapper& depth() const; const ImageWrapper& opticalFlow() const; const ImageWrapper& objectMotionMask() const; - const ImageWrapper& objectClassMask() const; const ImageWrapper& rightRgb() const; ImageWrapper& rgb(); ImageWrapper& depth(); ImageWrapper& opticalFlow(); ImageWrapper& objectMotionMask(); - ImageWrapper& objectClassMask(); ImageWrapper& rightRgb(); ImageContainer& rgb(const cv::Mat& image); ImageContainer& depth(const cv::Mat& image); ImageContainer& opticalFlow(const cv::Mat& image); ImageContainer& objectMotionMask(const cv::Mat& image); - ImageContainer& objectClassMask(const cv::Mat& image); ImageContainer& rightRgb(const cv::Mat& image); Timestamp timestamp() const { return timestamp_; } diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index efefe2e3f..91de1336a 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -63,10 +63,6 @@ const ImageWrapper& ImageContainer::objectMotionMask() return this->at(kObjectMask); } -const ImageWrapper& ImageContainer::objectClassMask() - const { - return this->at(kClassMask); - } const ImageWrapper& ImageContainer::rightRgb() const { return this->at(kRightRgb); @@ -85,10 +81,6 @@ ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } -ImageWrapper& ImageContainer::objectClassMask() { - return this->at(kClassMask); -} - ImageWrapper& ImageContainer::rightRgb() { return this->at(kRightRgb); } @@ -109,10 +101,6 @@ ImageContainer& ImageContainer::objectMotionMask(const cv::Mat& image) { return this->add(kObjectMask, image); } -ImageContainer& ImageContainer::objectClassMask(const cv::Mat& image) { - return this->add(kClassMask, image); -} - ImageContainer& ImageContainer::rightRgb(const cv::Mat& image) { return this->add(kRightRgb, image); } diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp index a203455c3..be8719665 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp @@ -84,18 +84,21 @@ class DSDTransport { static ObjectOdometry constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link, const std::string& child_frame_id_link); + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map); static ObjectOdometryMap constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link); + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map); static MultiObjectOdometryPath constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, const std::string& frame_id_link, - bool interpolate_missing_segments = true); + bool interpolate_missing_segments = true, + const ObjectIdClassMap& object_class_map = {}); /** * @brief Nested Publisher that publishes all the object odometries for a @@ -152,6 +155,8 @@ class DSDTransport { ObjectOdometryMap object_odometries_; MultiObjectOdometryPath object_paths_; + ObjectIdClassMap object_class_map_; + friend class DSDTransport; /** @@ -181,7 +186,8 @@ class DSDTransport { const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp); + Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); }; /** @@ -202,7 +208,8 @@ class DSDTransport { const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, - FrameId frame_id, Timestamp timestamp); + FrameId frame_id, Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); private: rclcpp::Node::SharedPtr node_; diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc index 5a180ee9a..0cf068766 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc @@ -31,7 +31,8 @@ std::string DSDTransport::constructObjectFrameLink(ObjectId object_id) { ObjectOdometry DSDTransport::constructObjectOdometry( const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link, const std::string& child_frame_id_link) { + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map) { ObjectOdometry object_odom; // technically this should be k-1 @@ -50,13 +51,24 @@ ObjectOdometry DSDTransport::constructObjectOdometry( object_odom.object_id = object_id; object_odom.sequence = frame_id_k; + + // Assigning the class label + auto observation = object_class_map.find(object_id); + if (observation != object_class_map.end()) { + object_odom.class_label=observation->second; + } else { + object_odom.class_label="unknown"; + } + + return object_odom; } ObjectOdometryMap DSDTransport::constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, - const std::string& frame_id_link) { + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map) { // need to get poses for k-1 // TODO: no way to ensure that the motions are for frame k // this is a weird data-structure to use and motions are per frame and @@ -85,7 +97,7 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( child_frame_id_link, constructObjectOdometry(e_H_k_world, pose_k, object_id, frame_id_k, timestamp_k, timestamp_km1, - frame_id_link, child_frame_id_link)); + frame_id_link, child_frame_id_link, object_class_map)); } return object_odom_map; @@ -94,7 +106,7 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, - const std::string& frame_id_link, bool interpolate_missing_segments) { + const std::string& frame_id_link, bool interpolate_missing_segments, const ObjectIdClassMap& object_class_map) { MultiObjectOdometryPath multi_path; multi_path.header.stamp = utils::toRosTime(timestamp_k); @@ -157,7 +169,7 @@ MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( gtsam::Pose3 motion; const ObjectOdometry object_odometry = constructObjectOdometry( object_motion, object_pose, object_id, frame_id, timestamp, timestamp_km1, - frame_id_link, child_frame_id_link); + frame_id_link, child_frame_id_link, object_class_map); if (!segmented_paths.exists(path_segment)) { ObjectOdometryPath path; @@ -239,7 +251,7 @@ void DSDTransport::Publisher::publishObjectPaths() { multi_object_odom_path_publisher_->publish(object_paths_); } -// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTEP ============================== +// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTAMP ============================== Timestamp getPrevTimestamp(const FrameIdTimestampMap& map, FrameId id, Timestamp current_timestamp) { if (id < 0) return current_timestamp; @@ -264,7 +276,8 @@ DSDTransport::Publisher::Publisher( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) : node_(node), object_odom_publisher_(object_odom_publisher), multi_object_odom_path_publisher_(multi_object_odom_path_publisher), @@ -274,19 +287,23 @@ DSDTransport::Publisher::Publisher( timestamp_(timestamp), timestamp_km1_(getPrevTimestamp(frame_timestamp_map, frame_id, timestamp)), // added to get (k-1)th timestamp object_odometries_(DSDTransport::constructObjectOdometries( - motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link)), + motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link, object_class_map)), object_paths_(DSDTransport::constructMultiObjectOdometryPaths( - motions, poses, timestamp, frame_timestamp_map, frame_id_link)) {} + motions, poses, timestamp, frame_timestamp_map, frame_id_link, true, object_class_map)), + object_class_map_(object_class_map){} + DSDTransport::Publisher DSDTransport::addObjectInfo( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, - const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) { + const FrameIdTimestampMap& frame_timestamp_map, + FrameId frame_id, + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) { return Publisher(node_, object_odom_publisher_, multi_object_odom_path_publisher_, tf_broadcaster_, motions, poses, frame_id_link, frame_timestamp_map, frame_id, - timestamp); + timestamp, object_class_map); } DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc index 670eb9216..e7099fa35 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc @@ -216,10 +216,11 @@ void FrontendDSDRos::tryPublishObjects( const auto& object_motions = object_motions_; const auto& object_poses = object_poses_; const auto& timestamp_map = this->shared_module_info.getTimestampMap(); + const auto& label_map = this->shared_module_info.getClassLabelMap(); DSDTransport::Publisher object_poses_publisher = dsd_transport_.addObjectInfo( object_motions, object_poses, params_.world_frame_id, timestamp_map, - frontend_output->frameId(), frontend_output->timestamp()); + frontend_output->frameId(), frontend_output->timestamp(), label_map); object_poses_publisher.publishObjectOdometry(); object_poses_publisher.publishObjectTransforms(); object_poses_publisher.publishObjectPaths(); diff --git a/dynosam_utils/src/install/.colcon_install_layout b/dynosam_utils/src/install/.colcon_install_layout new file mode 100644 index 000000000..3aad5336a --- /dev/null +++ b/dynosam_utils/src/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/dynosam_utils/src/install/COLCON_IGNORE b/dynosam_utils/src/install/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/install/_local_setup_util_ps1.py b/dynosam_utils/src/install/_local_setup_util_ps1.py new file mode 100644 index 000000000..3c6d9e877 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/_local_setup_util_sh.py b/dynosam_utils/src/install/_local_setup_util_sh.py new file mode 100644 index 000000000..f67eaa989 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/local_setup.bash b/dynosam_utils/src/install/local_setup.bash new file mode 100644 index 000000000..03f00256c --- /dev/null +++ b/dynosam_utils/src/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.ps1 b/dynosam_utils/src/install/local_setup.ps1 new file mode 100644 index 000000000..6f68c8ded --- /dev/null +++ b/dynosam_utils/src/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/dynosam_utils/src/install/local_setup.sh b/dynosam_utils/src/install/local_setup.sh new file mode 100644 index 000000000..e6ca4169e --- /dev/null +++ b/dynosam_utils/src/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/dev_ws/src/core/dynosam_utils/src/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.zsh b/dynosam_utils/src/install/local_setup.zsh new file mode 100644 index 000000000..b6487102f --- /dev/null +++ b/dynosam_utils/src/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.bash b/dynosam_utils/src/install/setup.bash new file mode 100644 index 000000000..396f8082a --- /dev/null +++ b/dynosam_utils/src/install/setup.bash @@ -0,0 +1,34 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/dynosam_utils/src/install/setup.ps1 b/dynosam_utils/src/install/setup.ps1 new file mode 100644 index 000000000..828baa9aa --- /dev/null +++ b/dynosam_utils/src/install/setup.ps1 @@ -0,0 +1,30 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/kilted\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/user/dev_ws/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/dynosam_utils/src/install/setup.sh b/dynosam_utils/src/install/setup.sh new file mode 100644 index 000000000..ad88868fb --- /dev/null +++ b/dynosam_utils/src/install/setup.sh @@ -0,0 +1,49 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/dev_ws/src/core/dynosam_utils/src/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.zsh b/dynosam_utils/src/install/setup.zsh new file mode 100644 index 000000000..fbb28ce08 --- /dev/null +++ b/dynosam_utils/src/install/setup.zsh @@ -0,0 +1,34 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/dynosam_utils/src/log/COLCON_IGNORE b/dynosam_utils/src/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log new file mode 100644 index 000000000..aac44b638 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000135] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log new file mode 100644 index 000000000..e731db663 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log @@ -0,0 +1,64 @@ +[0.220s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.220s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.273s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[0.326s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[0.327s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[0.479s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[0.480s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[0.496s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[0.507s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[0.508s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[0.518s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[0.519s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[0.521s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[0.523s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[0.525s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[0.526s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[0.527s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[0.529s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[0.530s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[0.531s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[0.532s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log new file mode 100644 index 000000000..d44325331 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000461] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log new file mode 100644 index 000000000..4dcaa6f21 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log @@ -0,0 +1,65 @@ +[0.726s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.726s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.882s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.882s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.884s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[1.029s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 10 installed packages in /home/user/dev_ws/install +[1.038s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[1.039s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[1.335s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[1.336s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.337s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.377s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.383s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[1.384s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.397s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[1.398s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[1.401s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[1.404s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[1.407s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[1.408s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[1.409s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[1.412s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[1.415s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[1.417s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[1.420s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/latest b/dynosam_utils/src/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/dynosam_utils/src/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/dynosam_utils/src/log/latest_build b/dynosam_utils/src/log/latest_build new file mode 120000 index 000000000..d799f4781 --- /dev/null +++ b/dynosam_utils/src/log/latest_build @@ -0,0 +1 @@ +build_2026-01-28_00-38-18 \ No newline at end of file diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py old mode 100644 new mode 100755 From c5d39891d788fbf3f40921150efdb28cb08ddfea Mon Sep 17 00:00:00 2001 From: pupueys Date: Thu, 26 Feb 2026 10:15:35 +1100 Subject: [PATCH 12/13] Cleaned up comments --- .../frontend/RGBDInstanceFrontendModule.cc | 1 - dynosam_common/src/DynamicObjects.cc | 19 +------------------ 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index 74952703a..628e1dc10 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -116,7 +116,6 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::boostrapSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container); - FrontendModule::FrameToClassMap(frame); CHECK(frame->updateDepths()); diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index 148cbd50c..a4f579586 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -74,8 +74,7 @@ std::ostream& operator<<(std::ostream& os, } -// OLD BODY MOTION CALCULATION IMPLEMENTATION -// want this to also output angular velocity too +// BODY MOTION CALCULATION IMPLEMENTATION gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, const gtsam::Pose3& w_L_k_1, Timestamp timestamp_km1, @@ -122,22 +121,6 @@ gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, return body_velocity; } -// gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, -// const gtsam::Pose3& w_L_k_1, -// Timestamp timestamp_km1, -// Timestamp timestamp_k) { - -// // Finding relative pose -// Timestamp dt = 0.04; // Estimated time between frames in kitti datasets - -// gtsam::Pose3 T_rel = w_L_k_1.inverse() * (w_k_1_H_k * w_L_k_1); // if pose is given in world frame -// // gtsam::Pose3 T_rel = w_k_1_H_k; // if pose is already in body frame - -// gtsam::Vector6 body_velocity = gtsam::Pose3::Logmap(T_rel) / dt; - -// return body_velocity; -// } - void propogateObjectPoses(ObjectPoseMap& object_poses, const MotionEstimateMap& object_motions_k, const gtsam::Point3Vector& object_centroids_k_1, From a01947d674121339bc721f6d5be43a486618ec99 Mon Sep 17 00:00:00 2001 From: pupueys Date: Thu, 26 Feb 2026 10:18:50 +1100 Subject: [PATCH 13/13] Deleted install files --- .../src/install/.colcon_install_layout | 1 - dynosam_utils/src/install/COLCON_IGNORE | 0 .../src/install/_local_setup_util_ps1.py | 407 ------------------ .../src/install/_local_setup_util_sh.py | 407 ------------------ dynosam_utils/src/install/local_setup.bash | 121 ------ dynosam_utils/src/install/local_setup.ps1 | 55 --- dynosam_utils/src/install/local_setup.sh | 137 ------ dynosam_utils/src/install/local_setup.zsh | 134 ------ dynosam_utils/src/install/setup.bash | 34 -- dynosam_utils/src/install/setup.ps1 | 30 -- dynosam_utils/src/install/setup.sh | 49 --- dynosam_utils/src/install/setup.zsh | 34 -- dynosam_utils/src/log/COLCON_IGNORE | 0 .../log/build_2026-01-27_04-03-52/events.log | 2 - .../build_2026-01-27_04-03-52/logger_all.log | 64 --- .../log/build_2026-01-28_00-38-18/events.log | 2 - .../build_2026-01-28_00-38-18/logger_all.log | 65 --- dynosam_utils/src/log/latest | 1 - dynosam_utils/src/log/latest_build | 1 - 19 files changed, 1544 deletions(-) delete mode 100644 dynosam_utils/src/install/.colcon_install_layout delete mode 100644 dynosam_utils/src/install/COLCON_IGNORE delete mode 100644 dynosam_utils/src/install/_local_setup_util_ps1.py delete mode 100644 dynosam_utils/src/install/_local_setup_util_sh.py delete mode 100644 dynosam_utils/src/install/local_setup.bash delete mode 100644 dynosam_utils/src/install/local_setup.ps1 delete mode 100644 dynosam_utils/src/install/local_setup.sh delete mode 100644 dynosam_utils/src/install/local_setup.zsh delete mode 100644 dynosam_utils/src/install/setup.bash delete mode 100644 dynosam_utils/src/install/setup.ps1 delete mode 100644 dynosam_utils/src/install/setup.sh delete mode 100644 dynosam_utils/src/install/setup.zsh delete mode 100644 dynosam_utils/src/log/COLCON_IGNORE delete mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log delete mode 100644 dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log delete mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log delete mode 100644 dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log delete mode 120000 dynosam_utils/src/log/latest delete mode 120000 dynosam_utils/src/log/latest_build diff --git a/dynosam_utils/src/install/.colcon_install_layout b/dynosam_utils/src/install/.colcon_install_layout deleted file mode 100644 index 3aad5336a..000000000 --- a/dynosam_utils/src/install/.colcon_install_layout +++ /dev/null @@ -1 +0,0 @@ -isolated diff --git a/dynosam_utils/src/install/COLCON_IGNORE b/dynosam_utils/src/install/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/dynosam_utils/src/install/_local_setup_util_ps1.py b/dynosam_utils/src/install/_local_setup_util_ps1.py deleted file mode 100644 index 3c6d9e877..000000000 --- a/dynosam_utils/src/install/_local_setup_util_ps1.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' -FORMAT_STR_USE_ENV_VAR = '$env:{name}' -FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/dynosam_utils/src/install/_local_setup_util_sh.py b/dynosam_utils/src/install/_local_setup_util_sh.py deleted file mode 100644 index f67eaa989..000000000 --- a/dynosam_utils/src/install/_local_setup_util_sh.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' -FORMAT_STR_USE_ENV_VAR = '${name}' -FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/dynosam_utils/src/install/local_setup.bash b/dynosam_utils/src/install/local_setup.bash deleted file mode 100644 index 03f00256c..000000000 --- a/dynosam_utils/src/install/local_setup.bash +++ /dev/null @@ -1,121 +0,0 @@ -# generated from colcon_bash/shell/template/prefix.bash.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -else - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_bash_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" - unset _colcon_prefix_bash_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_bash_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.ps1 b/dynosam_utils/src/install/local_setup.ps1 deleted file mode 100644 index 6f68c8ded..000000000 --- a/dynosam_utils/src/install/local_setup.ps1 +++ /dev/null @@ -1,55 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix.ps1.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# check environment variable for custom Python executable -if ($env:COLCON_PYTHON_EXECUTABLE) { - if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { - echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" - exit 1 - } - $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" -} else { - # use the Python executable known at configure time - $_colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { - if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { - echo "error: unable to find python3 executable" - exit 1 - } - $_colcon_python_executable="python3" - } -} - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_powershell_source_script { - param ( - $_colcon_prefix_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_powershell_source_script_param'" - } - . "$_colcon_prefix_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" - } -} - -# get all commands in topological order -$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 - -# execute all commands in topological order -if ($env:COLCON_TRACE) { - echo "Execute generated script:" - echo "<<<" - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output - echo ">>>" -} -if ($_colcon_ordered_commands) { - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression -} diff --git a/dynosam_utils/src/install/local_setup.sh b/dynosam_utils/src/install/local_setup.sh deleted file mode 100644 index e6ca4169e..000000000 --- a/dynosam_utils/src/install/local_setup.sh +++ /dev/null @@ -1,137 +0,0 @@ -# generated from colcon_core/shell/template/prefix.sh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/dev_ws/src/core/dynosam_utils/src/install" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX - return 1 - fi -else - _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_sh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" - unset _colcon_prefix_sh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_sh_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "_colcon_prefix_sh_source_script() { - if [ -f \"\$1\" ]; then - if [ -n \"\$COLCON_TRACE\" ]; then - echo \"# . \\\"\$1\\\"\" - fi - . \"\$1\" - else - echo \"not found: \\\"\$1\\\"\" 1>&2 - fi - }" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.zsh b/dynosam_utils/src/install/local_setup.zsh deleted file mode 100644 index b6487102f..000000000 --- a/dynosam_utils/src/install/local_setup.zsh +++ /dev/null @@ -1,134 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix.zsh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -else - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -_colcon_prefix_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_zsh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # workaround SH_WORD_SPLIT not being set - _colcon_prefix_zsh_convert_to_array _values - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" - unset _colcon_prefix_zsh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_zsh_prepend_unique_value -unset _colcon_prefix_zsh_convert_to_array - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.bash b/dynosam_utils/src/install/setup.bash deleted file mode 100644 index 396f8082a..000000000 --- a/dynosam_utils/src/install/setup.bash +++ /dev/null @@ -1,34 +0,0 @@ -# generated from colcon_bash/shell/template/prefix_chain.bash.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/kilted" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script diff --git a/dynosam_utils/src/install/setup.ps1 b/dynosam_utils/src/install/setup.ps1 deleted file mode 100644 index 828baa9aa..000000000 --- a/dynosam_utils/src/install/setup.ps1 +++ /dev/null @@ -1,30 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix_chain.ps1.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_chain_powershell_source_script { - param ( - $_colcon_prefix_chain_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_chain_powershell_source_script_param'" - } - . "$_colcon_prefix_chain_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" - } -} - -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/kilted\local_setup.ps1" -_colcon_prefix_chain_powershell_source_script "/home/user/dev_ws/install\local_setup.ps1" - -# source this prefix -$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) -_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/dynosam_utils/src/install/setup.sh b/dynosam_utils/src/install/setup.sh deleted file mode 100644 index ad88868fb..000000000 --- a/dynosam_utils/src/install/setup.sh +++ /dev/null @@ -1,49 +0,0 @@ -# generated from colcon_core/shell/template/prefix_chain.sh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/dev_ws/src/core/dynosam_utils/src/install -if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX - return 1 -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/kilted" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_sh_source_script -unset COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.zsh b/dynosam_utils/src/install/setup.zsh deleted file mode 100644 index fbb28ce08..000000000 --- a/dynosam_utils/src/install/setup.zsh +++ /dev/null @@ -1,34 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix_chain.zsh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/kilted" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script diff --git a/dynosam_utils/src/log/COLCON_IGNORE b/dynosam_utils/src/log/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log deleted file mode 100644 index aac44b638..000000000 --- a/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log +++ /dev/null @@ -1,2 +0,0 @@ -[0.000000] (-) TimerEvent: {} -[0.000135] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log deleted file mode 100644 index e731db663..000000000 --- a/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log +++ /dev/null @@ -1,64 +0,0 @@ -[0.220s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] -[0.220s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover -[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.273s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] -[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' -[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] -[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' -[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' -[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] -[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' -[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored -[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults -[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover -[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults -[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover -[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults -[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters -[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover -[0.326s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted -[0.327s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults -[0.479s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor -[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete -[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop -[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed -[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' -[0.480s] DEBUG:colcon.colcon_core.event_reactor:joining thread -[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' -[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems -[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems -[0.496s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' -[0.507s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message -[0.508s] DEBUG:colcon.colcon_core.event_reactor:joined thread -[0.518s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems -[0.519s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' -[0.521s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' -[0.523s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' -[0.525s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' -[0.526s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' -[0.527s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' -[0.529s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' -[0.530s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' -[0.531s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' -[0.532s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log deleted file mode 100644 index d44325331..000000000 --- a/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log +++ /dev/null @@ -1,2 +0,0 @@ -[0.000000] (-) TimerEvent: {} -[0.000461] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log deleted file mode 100644 index 4dcaa6f21..000000000 --- a/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log +++ /dev/null @@ -1,65 +0,0 @@ -[0.726s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] -[0.726s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) -[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover -[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.882s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' -[0.882s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' -[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] -[0.884s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' -[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] -[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' -[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' -[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] -[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' -[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored -[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults -[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover -[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults -[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover -[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults -[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters -[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover -[1.029s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 10 installed packages in /home/user/dev_ws/install -[1.038s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted -[1.039s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults -[1.335s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor -[1.336s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete -[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop -[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed -[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' -[1.337s] DEBUG:colcon.colcon_core.event_reactor:joining thread -[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' -[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems -[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems -[1.377s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' -[1.383s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message -[1.384s] DEBUG:colcon.colcon_core.event_reactor:joined thread -[1.397s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems -[1.398s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' -[1.401s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' -[1.404s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' -[1.407s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' -[1.408s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' -[1.409s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' -[1.412s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' -[1.415s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' -[1.417s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' -[1.420s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/latest b/dynosam_utils/src/log/latest deleted file mode 120000 index b57d247c7..000000000 --- a/dynosam_utils/src/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_build \ No newline at end of file diff --git a/dynosam_utils/src/log/latest_build b/dynosam_utils/src/log/latest_build deleted file mode 120000 index d799f4781..000000000 --- a/dynosam_utils/src/log/latest_build +++ /dev/null @@ -1 +0,0 @@ -build_2026-01-28_00-38-18 \ No newline at end of file