Skip to content

Commit

Permalink
Add extrinsic imu calibration
Browse files Browse the repository at this point in the history
  • Loading branch information
Flova committed Mar 20, 2024
1 parent 98a71dd commit c140489
Show file tree
Hide file tree
Showing 11 changed files with 131 additions and 105 deletions.
2 changes: 0 additions & 2 deletions bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,6 @@ find_package(tf2 REQUIRED)
find_package(rot_conv REQUIRED)
find_package(backward_ros REQUIRED)

include_directories(include)

add_compile_options(-Wall -Werror -Wno-unused)

add_executable(extrinsic_calibration src/extrinsic_calibration.cpp)
Expand Down
8 changes: 7 additions & 1 deletion bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,10 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,10 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
Expand Down
8 changes: 7 additions & 1 deletion bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,10 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
Expand Down
8 changes: 7 additions & 1 deletion bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml
Original file line number Diff line number Diff line change
@@ -1,5 +1,11 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: -0.05
offset_y: 0.15
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
offset_z: 0.0
Original file line number Diff line number Diff line change
@@ -1,4 +1,10 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: 0.0
offset_y: 0.0
Expand Down
10 changes: 8 additions & 2 deletions bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml
Original file line number Diff line number Diff line change
@@ -1,5 +1,11 @@
/bitbots_extrinsic_calibration:
/bitbots_extrinsic_camera_calibration:
ros__parameters:
offset_x: 0.0
offset_x: -0.03
offset_y: 0.0
offset_z: 0.0

/bitbots_extrinsic_imu_calibration:
ros__parameters:
offset_x: -0.03
offset_y: 0.03
offset_z: 0.0

This file was deleted.

Original file line number Diff line number Diff line change
@@ -1,8 +1,16 @@
<launch>
<arg name="sim" default="false"/>

<node pkg="bitbots_extrinsic_calibration" exec="extrinsic_calibration" name="bitbots_extrinsic_calibration">
<node pkg="bitbots_extrinsic_calibration" exec="extrinsic_calibration" name="bitbots_extrinsic_camera_calibration">
<param name="use_sim_time" value="$(var sim)"/>
<param name="parent_frame" value="camera_optical_frame_uncalibrated"/>
<param name="child_frame" value="camera_optical_frame"/>
<param from="$(find-pkg-share bitbots_extrinsic_calibration)/config/$(env ROBOT_NAME default).yaml" />
</node>
<node pkg="bitbots_extrinsic_calibration" exec="extrinsic_calibration" name="bitbots_extrinsic_imu_calibration">
<param name="use_sim_time" value="$(var sim)"/>
<param name="parent_frame" value="imu_frame_uncalibrated"/>
<param name="child_frame" value="imu_frame"/>
<param from="$(find-pkg-share bitbots_extrinsic_calibration)/config/$(env ROBOT_NAME default).yaml" />
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -1,74 +1,82 @@
#include <extrinsic_calibration/extrinsic_calibration.hpp>

ExtrinsicCalibrationBroadcaster::ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") {
broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);

this->declare_parameter<std::string>("parent_frame", "camera");
this->declare_parameter<std::string>("child_frame", "camera_optical_frame");
this->declare_parameter<double>("offset_x", 0.0, rcl_interfaces::msg::ParameterDescriptor());
this->declare_parameter<double>("offset_y", 0.0);
this->declare_parameter<double>("offset_z", 0.0);

auto parameters = this->get_parameters(this->list_parameters({}, 10).names);
ExtrinsicCalibrationBroadcaster::onSetParameters(parameters);

param_callback_handle_ =
this->add_on_set_parameters_callback(std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1));
}

rcl_interfaces::msg::SetParametersResult ExtrinsicCalibrationBroadcaster::onSetParameters(
const std::vector<rclcpp::Parameter> &parameters) {
for (const auto &parameter : parameters) {
if (parameter.get_name() == "offset_x") {
offset_x_ = parameter.as_double();
} else if (parameter.get_name() == "offset_y") {
offset_y_ = parameter.as_double();
} else if (parameter.get_name() == "offset_z") {
offset_z_ = parameter.as_double();
} else if (parameter.get_name() == "parent_frame") {
parent_frame_ = parameter.as_string();
} else if (parameter.get_name() == "child_frame") {
child_frame_ = parameter.as_string();
} else {
RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str());
}
#include <rot_conv/rot_conv.h>
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/char.hpp>
#include <utility>
using std::placeholders::_1;

class ExtrinsicCalibrationBroadcaster : public rclcpp::Node {
public:
ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") {
broadcaster_ = std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);

this->declare_parameter<std::string>("parent_frame", "camera_optical_frame_uncalibrated");
this->declare_parameter<std::string>("child_frame", "camera_optical_frame");
this->declare_parameter<double>("offset_x", 0.0, rcl_interfaces::msg::ParameterDescriptor());
this->declare_parameter<double>("offset_y", 0.0);
this->declare_parameter<double>("offset_z", 0.0);

auto parameters = this->get_parameters(this->list_parameters({}, 10).names);
onSetParameters(parameters);

param_callback_handle_ =
this->add_on_set_parameters_callback(std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1));
}

auto base_quat = rot_conv::QuatFromEuler(-1.5708, 0.0, -1.5708);
auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_);

auto final_quat = offset_quat * base_quat;
rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector<rclcpp::Parameter> &parameters) {
for (const auto &parameter : parameters) {
if (parameter.get_name() == "offset_x") {
offset_x_ = parameter.as_double();
} else if (parameter.get_name() == "offset_y") {
offset_y_ = parameter.as_double();
} else if (parameter.get_name() == "offset_z") {
offset_z_ = parameter.as_double();
} else if (parameter.get_name() == "parent_frame") {
parent_frame_ = parameter.as_string();
} else if (parameter.get_name() == "child_frame") {
child_frame_ = parameter.as_string();
} else {
RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str());
}
}
auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_);

transform_.rotation.x = final_quat.x();
transform_.rotation.y = final_quat.y();
transform_.rotation.z = final_quat.z();
transform_.rotation.w = final_quat.w();
transform_.rotation.x = offset_quat.x();
transform_.rotation.y = offset_quat.y();
transform_.rotation.z = offset_quat.z();
transform_.rotation.w = offset_quat.w();

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = this->now();
tf.header.frame_id = parent_frame_;
tf.child_frame_id = child_frame_;
tf.transform = transform_;
broadcaster_->sendTransform(tf);

return result;
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;

void ExtrinsicCalibrationBroadcaster::step() {
rclcpp::Time now = this->now();
return result;
}

geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = now;
tf.header.frame_id = parent_frame_;
tf.child_frame_id = child_frame_;
tf.transform = transform_;
broadcaster_->sendTransform(tf);
}
private:
OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
geometry_msgs::msg::Transform transform_{geometry_msgs::msg::Transform()};
std::string parent_frame_, child_frame_;
double offset_x_ = 0, offset_y_ = 0, offset_z_ = 0;
};

int main(int argc, char **argv) {
rclcpp::init(argc, argv);

auto node = std::make_shared<ExtrinsicCalibrationBroadcaster>();

rclcpp::TimerBase::SharedPtr timer =
rclcpp::create_timer(node, node->get_clock(), rclcpp::Duration(0, 1e7), [node]() -> void { node->step(); });

rclcpp::experimental::executors::EventsExecutor exec;
exec.add_node(node);
exec.spin();
Expand Down
25 changes: 13 additions & 12 deletions bitbots_wolfgang/wolfgang_description/urdf/robot.urdf
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,7 @@
<color rgba="0.647059 0.647059 0.647059 1.0"/>
</material>
</visual>

<visual>
<origin rpy="-3.14159 1.5708 0.0" xyz="-0.0015 -0.036 0.2035"/>
<geometry>
Expand Down Expand Up @@ -374,18 +374,19 @@
</material>
</visual>
</link>
<link name="imu_frame">
<!-- Renamed due to extrinsic calibration -->
<link name="imu_frame_uncalibrated">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="1e-09"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<joint name="imu_frame_frame" type="fixed">
<joint name="imu_frame_uncalibrated_frame" type="fixed">
<origin rpy="5.31321e-16 -3.12873e-17 4.65401e-17" xyz="-0.0369749 -0.0474614 0.224"/>
<axis xyz="0.0 0.0 0.0"/>
<parent link="torso"/>
<child link="imu_frame"/>
<child link="imu_frame_uncalibrated"/>
</joint>
<link name="r_shoulder">
<inertial>
Expand Down Expand Up @@ -3588,21 +3589,21 @@
<child link="camera"/>
</joint>
<!--
Removed due to extrinsic calibration
<link name="camera_optical_frame">
Renamed due to extrinsic calibration
-->
<link name="camera_optical_frame_uncalibrated">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="1e-09"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<joint name="camera_optical_frame_frame" type="fixed">
<joint name="camera_optical_frame_uncalibrated_frame" type="fixed">
<origin rpy="3.14159 -1.5708 0.0" xyz="0.0432 0.08 -0.0235"/>
<axis xyz="0.0 0.0 0.0"/>
<parent link="head"/>
<child link="camera_optical_frame"/>
<child link="camera_optical_frame_uncalibrated"/>
</joint>
-->
<link name="imu_frame_2">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
Expand Down Expand Up @@ -3870,7 +3871,7 @@
</plugin>
</sensor>
</gazebo>
<gazebo reference="imu_frame">
<gazebo reference="imu_frame_uncalibrated">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
Expand All @@ -3879,12 +3880,12 @@
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>imu_frame</bodyName>
<bodyName>imu_frame_uncalibrated</bodyName>
<updateRateHZ>100.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_frame</frameName>
<frameName>imu_frame_uncalibrated</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
Expand Down

0 comments on commit c140489

Please sign in to comment.