diff --git a/README.md b/README.md index 6eafd13..ca17395 100644 --- a/README.md +++ b/README.md @@ -71,6 +71,11 @@ void Example::onJoyCallback(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg) { | L2 | `float` | `pressedL2Analog();` | `-1.0` : Pressed, `1.0` : Not pressed | +## Convert from Left Joy Stick to cmd_vel +`teleop_twist_joy_node` has cmd_vel publisher. +Velocity is decided as below. +![Image](./media/joy2vel.png) + ## Acknowledgements We acknowledge attribute and gratitude to the following resources in creating this package. diff --git a/media/joy2vel.png b/media/joy2vel.png new file mode 100644 index 0000000..88e5dda Binary files /dev/null and b/media/joy2vel.png differ diff --git a/p9n_node/include/p9n_node/teleop_twist_joy_node.hpp b/p9n_node/include/p9n_node/teleop_twist_joy_node.hpp index 5858b0f..2b396b5 100644 --- a/p9n_node/include/p9n_node/teleop_twist_joy_node.hpp +++ b/p9n_node/include/p9n_node/teleop_twist_joy_node.hpp @@ -32,7 +32,6 @@ class TeleopTwistJoyNode : public rclcpp::Node using Joy = sensor_msgs::msg::Joy; private: - float speed_factor_ = 1.0; double linear_max_speed_, angular_max_speed_; p9n_interface::HW_TYPE hw_type_; diff --git a/p9n_node/src/teleop_twist_joy_node.cpp b/p9n_node/src/teleop_twist_joy_node.cpp index 7f45f4d..04cdb77 100644 --- a/p9n_node/src/teleop_twist_joy_node.cpp +++ b/p9n_node/src/teleop_twist_joy_node.cpp @@ -69,10 +69,13 @@ void TeleopTwistJoyNode::onJoy(Joy::ConstSharedPtr joy_msg) auto twist_msg = std::make_unique(); twist_msg->linear.set__x(this->linear_max_speed_ * this->p9n_if_->tiltedStickLY()); - if (twist_msg->linear.x > 0) { + if (this->p9n_if_->tiltedStickLY() > sin(M_PI * 0.125)) { twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX()); - } else { + } else if (this->p9n_if_->tiltedStickLY() < sin(-M_PI * 0.125)) { twist_msg->angular.set__z(-this->angular_max_speed_ * this->p9n_if_->tiltedStickLX()); + } else { + twist_msg->linear.set__x(0.0); + twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX()); } this->twist_pub_->publish(std::move(twist_msg));