Skip to content

Commit

Permalink
refactor(pose2twist): apply static analysis (#7307)
Browse files Browse the repository at this point in the history
* refactor based on linter

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* add cast double to float

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

* remove default destructor

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

---------

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>
  • Loading branch information
a-maumau authored Jun 7, 2024
1 parent 321e1c7 commit e800d54
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -25,10 +25,9 @@ class Pose2Twist : public rclcpp::Node
{
public:
explicit Pose2Twist(const rclcpp::NodeOptions & options);
~Pose2Twist() = default;

private:
void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
void callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;

Expand Down
32 changes: 16 additions & 16 deletions localization/pose2twist/src/pose2twist_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,10 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose
create_publisher<tier4_debug_msgs::msg::Float32Stamped>("angular_z", durable_qos);
// Note: this callback publishes topics above
pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1));
"pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1));
}

double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
double calc_diff_for_radian(const double lhs_rad, const double rhs_rad)
{
double diff_rad = lhs_rad - rhs_rad;
if (diff_rad > M_PI) {
Expand All @@ -53,20 +53,20 @@ double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
}

// x: roll, y: pitch, z: yaw
geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose)
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose)
{
geometry_msgs::msg::Vector3 rpy;
tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
return rpy;
}

geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose)
geometry_msgs::msg::Vector3 get_rpy(geometry_msgs::msg::PoseStamped::SharedPtr pose)
{
return getRPY(pose->pose);
return get_rpy(pose->pose);
}

geometry_msgs::msg::TwistStamped calcTwist(
geometry_msgs::msg::TwistStamped calc_twist(
geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
{
Expand All @@ -79,18 +79,18 @@ geometry_msgs::msg::TwistStamped calcTwist(
return twist;
}

const auto pose_a_rpy = getRPY(pose_a);
const auto pose_b_rpy = getRPY(pose_b);
const auto pose_a_rpy = get_rpy(pose_a);
const auto pose_b_rpy = get_rpy(pose_b);

geometry_msgs::msg::Vector3 diff_xyz;
geometry_msgs::msg::Vector3 diff_rpy;

diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);
diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x);
diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y);
diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z);

geometry_msgs::msg::TwistStamped twist;
twist.header = pose_b->header;
Expand All @@ -106,27 +106,27 @@ geometry_msgs::msg::TwistStamped calcTwist(
return twist;
}

void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
{
// TODO(YamatoAndo) check time stamp diff
// TODO(YamatoAndo) check suddenly move
// TODO(YamatoAndo) apply low pass filter

geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr;
const geometry_msgs::msg::PoseStamped::SharedPtr & current_pose_msg = pose_msg_ptr;
static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg;
geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg);
geometry_msgs::msg::TwistStamped twist_msg = calc_twist(prev_pose_msg, current_pose_msg);
prev_pose_msg = current_pose_msg;
twist_msg.header.frame_id = "base_link";
twist_pub_->publish(twist_msg);

tier4_debug_msgs::msg::Float32Stamped linear_x_msg;
linear_x_msg.stamp = this->now();
linear_x_msg.data = twist_msg.twist.linear.x;
linear_x_msg.data = static_cast<float>(twist_msg.twist.linear.x);
linear_x_pub_->publish(linear_x_msg);

tier4_debug_msgs::msg::Float32Stamped angular_z_msg;
angular_z_msg.stamp = this->now();
angular_z_msg.data = twist_msg.twist.angular.z;
angular_z_msg.data = static_cast<float>(twist_msg.twist.angular.z);
angular_z_pub_->publish(angular_z_msg);
}

Expand Down

0 comments on commit e800d54

Please sign in to comment.