-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
423dd8f
commit 893c8d7
Showing
27 changed files
with
2,026 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,7 @@ | ||
| #!/bin/sh | ||
| set -x | ||
| sudo apt-get update && sudo apt-get install libgl1-mesa-glx xvfb -y | ||
| Xvfb :1 -screen 0 1024x768x24 > /dev/null 2>&1 & | ||
| # give xvfb some time to start | ||
| sleep 3 | ||
| set +x |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -23,6 +23,7 @@ jobs: | |
| image: robotlocomotion/drake:${{matrix.os_code_name}} | ||
| env: | ||
| PYTHONPATH: | ||
| DISPLAY: ':1.0' | ||
| steps: | ||
| - uses: actions/checkout@v4 | ||
| - name: Simplify apt upgrades | ||
|
|
@@ -33,6 +34,8 @@ jobs: | |
| use-ros2-testing: "true" | ||
| - name: Cope with Python 2 pollution | ||
| run: apt-get update && apt-get install -y python-is-python3 | ||
| - name: Configure display | ||
| run: .github/config_display.sh | ||
| - name: Build and test all packages | ||
| uses: ros-tooling/[email protected] | ||
| with: | ||
|
|
@@ -51,6 +54,7 @@ jobs: | |
| image: robotlocomotion/drake:${{matrix.os_code_name}} | ||
| env: | ||
| PYTHONPATH: | ||
| DISPLAY: ':1.0' | ||
| steps: | ||
| - uses: actions/checkout@v4 | ||
| - name: Simplify apt upgrades | ||
|
|
@@ -80,5 +84,6 @@ jobs: | |
| colcon build --event-handlers console_cohesion+ | ||
| - name: Test all packages | ||
| run: | | ||
| .github/config_display.sh | ||
| . /opt/ros/${{matrix.ros_distro}}/setup.sh | ||
| colcon test --event-handlers console_cohesion+ | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,85 @@ | ||
| #include "drake_ros/core/camera_info_system.h" | ||
|
|
||
| #include <rclcpp/time.hpp> | ||
|
|
||
| using drake_ros::core::CameraInfoSystem; | ||
| using drake_ros::core::RosPublisherSystem; | ||
|
|
||
| CameraInfoSystem::CameraInfoSystem() : camera_info(1, 1, 1, 1, 0.5, 0.5) { | ||
| DeclareAbstractOutputPort("CameraInfoSystem", | ||
| &CameraInfoSystem::CalcCameraInfo); | ||
| } | ||
|
|
||
| CameraInfoSystem::~CameraInfoSystem() {} | ||
|
|
||
| void CameraInfoSystem::CalcCameraInfo( | ||
| const drake::systems::Context<double>& context, | ||
| sensor_msgs::msg::CameraInfo* output_value) const { | ||
| rclcpp::Time now{0, 0, RCL_ROS_TIME}; | ||
| now += rclcpp::Duration::from_seconds(context.get_time()); | ||
| output_value->header.stamp = now; | ||
| output_value->header.frame_id = "CartPole/camera_optical"; | ||
|
|
||
| output_value->height = this->camera_info.height(); | ||
| output_value->width = this->camera_info.width(); | ||
| output_value->distortion_model = "plumb_bob"; | ||
|
|
||
| // distortion isn't supported. Setting this values to zero | ||
| output_value->d.push_back(0); | ||
| output_value->d.push_back(0); | ||
| output_value->d.push_back(0); | ||
| output_value->d.push_back(0); | ||
| output_value->d.push_back(0); | ||
|
|
||
| output_value->r[0] = 1; | ||
| output_value->r[1] = 0; | ||
| output_value->r[2] = 0; | ||
| output_value->r[3] = 0; | ||
| output_value->r[4] = 1; | ||
| output_value->r[5] = 0; | ||
| output_value->r[6] = 0; | ||
| output_value->r[7] = 0; | ||
| output_value->r[8] = 1; | ||
|
|
||
| // │ f_x 0 c_x │ | ||
| // K = │ 0 f_y c_y │ | ||
| // │ 0 0 1 │ | ||
|
|
||
| // │ f_x 0 c_x Tx│ | ||
| // P = │ 0 f_y c_y Ty│ | ||
| // │ 0 0 1 0│ | ||
| output_value->k[0] = this->camera_info.focal_x(); | ||
| output_value->k[2] = this->camera_info.center_x(); | ||
| output_value->k[4] = this->camera_info.focal_y(); | ||
| output_value->k[5] = this->camera_info.center_y(); | ||
| output_value->k[8] = 1.0; | ||
|
|
||
| output_value->p[0] = this->camera_info.focal_x(); | ||
| output_value->p[2] = this->camera_info.center_x(); | ||
| output_value->p[3] = 0; | ||
| output_value->p[5] = this->camera_info.focal_y(); | ||
| output_value->p[6] = this->camera_info.center_y(); | ||
| output_value->p[10] = 1.0; | ||
| } | ||
|
|
||
| void CameraInfoSystem::SetCameraInfo(const CameraInfo& _camera_info) { | ||
| this->camera_info = _camera_info; | ||
| } | ||
|
|
||
| std::tuple<CameraInfoSystem*, RosPublisherSystem*> | ||
| CameraInfoSystem::AddToBuilder( | ||
| drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros, | ||
| const std::string& topic_name, const rclcpp::QoS& qos, | ||
| const std::unordered_set<drake::systems::TriggerType>& publish_triggers, | ||
| double publish_period) { | ||
| auto* camera_info_system = builder->AddSystem<CameraInfoSystem>(); | ||
|
|
||
| auto* pub_system = | ||
| builder->AddSystem(RosPublisherSystem::Make<sensor_msgs::msg::CameraInfo>( | ||
| topic_name, qos, ros, publish_triggers, publish_period)); | ||
|
|
||
| builder->Connect(camera_info_system->get_output_port(), | ||
| pub_system->get_input_port()); | ||
|
|
||
| return {camera_info_system, pub_system}; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,53 @@ | ||
| #pragma once | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
| #include <tuple> | ||
| #include <unordered_set> | ||
|
|
||
| #include <drake/systems/framework/diagram_builder.h> | ||
| #include <drake/systems/framework/leaf_system.h> | ||
| #include <drake/systems/sensors/camera_info.h> | ||
| #include <drake_ros/core/ros_publisher_system.h> | ||
| #include <rclcpp/qos.hpp> | ||
| #include <sensor_msgs/msg/camera_info.hpp> | ||
|
|
||
| using drake::systems::sensors::CameraInfo; | ||
|
|
||
| namespace drake_ros::core { | ||
| /** A system that converts drake's camera info to a sensor_msgs/msg/CameraInfo | ||
| * message. | ||
| */ | ||
| class CameraInfoSystem : public drake::systems::LeafSystem<double> { | ||
| public: | ||
| /** A constructor for the camera info system. | ||
| */ | ||
| CameraInfoSystem(); | ||
|
|
||
| ~CameraInfoSystem() override; | ||
|
|
||
| void SetCameraInfo(const CameraInfo& _camera_info); | ||
|
|
||
| /** Add a CameraInfoSystem and RosPublisherSystem to a diagram builder. | ||
| * | ||
| * This adds both a CameraInfoSystem and a RosPublisherSystem that publishes | ||
| * time to a `/image/camera_info` topic. All nodes should have their | ||
| * `use_sim_time` parameter set to `True` so they use the published topic as | ||
| * their source of time. | ||
| */ | ||
| static std::tuple<CameraInfoSystem*, RosPublisherSystem*> AddToBuilder( | ||
| drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros, | ||
| const std::string& topic_name = "/image/camera_info", | ||
| const rclcpp::QoS& qos = rclcpp::SystemDefaultsQoS(), | ||
| const std::unordered_set<drake::systems::TriggerType>& publish_triggers = | ||
| RosPublisherSystem::kDefaultTriggerTypes, | ||
| double publish_period = 0.0); | ||
|
|
||
| private: | ||
| CameraInfo camera_info; | ||
|
|
||
| protected: | ||
| void CalcCameraInfo(const drake::systems::Context<double>& context, | ||
| sensor_msgs::msg::CameraInfo* output_value) const; | ||
| }; | ||
| } // namespace drake_ros::core |
Oops, something went wrong.