Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

joystick_interrupt: enable omni-directional movement #645

Merged
merged 2 commits into from
Jul 26, 2022
Merged

joystick_interrupt: enable omni-directional movement #645

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4e683b9
Enable omni-directional movement using joystick_interrupt
nhatao Jul 26, 2022
8b12e4b
Fix roslint error
nhatao Jul 26, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 39 additions & 38 deletions joystick_interrupt/src/joystick_interrupt.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
*/

#include <cmath>
#include <string>

#include <ros/ros.h>

Expand All @@ -48,18 +49,44 @@ class JoystickInterrupt
ros::Publisher pub_int_;
double linear_vel_;
double angular_vel_;
double linear_y_vel_;
double timeout_;
double linear_high_speed_ratio_;
double angular_high_speed_ratio_;
int linear_axis_;
int angular_axis_;
int linear_axis2_;
int angular_axis2_;
int linear_y_axis_;
int linear_y_axis2_;
int interrupt_button_;
int high_speed_button_;
ros::Time last_joy_msg_;
geometry_msgs::Twist last_input_twist_;

float getAxisValue(const sensor_msgs::Joy::Ptr& msg, const int axis, const std::string& axis_name) const
{
if (axis < 0)
{
return 0.0;
}
if (static_cast<size_t>(axis) >= msg->axes.size())
{
ROS_ERROR("Out of range: number of axis (%lu) must be greater than %s (%d).",
msg->axes.size(), axis_name.c_str(), axis);
return 0.0;
}
return msg->axes[axis];
}

float getJoyValue(const sensor_msgs::Joy::Ptr& msg, const int axis, const int axis2,
const std::string& axis_name) const
{
const float value = getAxisValue(msg, axis, axis_name);
const float value2 = getAxisValue(msg, axis2, axis_name + "2");
return (std::abs(value2) > std::abs(value)) ? value2 : value;
}

void cbJoy(const sensor_msgs::Joy::Ptr msg)
{
if (static_cast<size_t>(interrupt_button_) >= msg->buttons.size())
Expand All @@ -81,49 +108,18 @@ class JoystickInterrupt

last_joy_msg_ = ros::Time::now();

float lin(0.0f);
float ang(0.0f);
if (static_cast<size_t>(linear_axis_) < msg->axes.size())
lin = msg->axes[linear_axis_];
else
ROS_ERROR("Out of range: number of axis (%lu) must be greater than linear_axis (%d).",
msg->axes.size(), linear_axis_);
if (static_cast<size_t>(angular_axis_) < msg->axes.size())
ang = msg->axes[angular_axis_];
else
ROS_ERROR("Out of range: number of axis (%lu) must be greater than angular_axis (%d).",
msg->axes.size(), angular_axis_);

if (linear_axis2_ >= 0)
{
if (static_cast<size_t>(linear_axis2_) < msg->axes.size())
{
if (std::abs(msg->axes[linear_axis2_]) > std::abs(lin))
lin = msg->axes[linear_axis2_];
}
else
ROS_ERROR("Out of range: number of axis (%lu) must be greater than linear_axis2 (%d).",
msg->axes.size(), linear_axis2_);
}
if (angular_axis2_ >= 0)
{
if (static_cast<size_t>(angular_axis2_) < msg->axes.size())
{
if (std::abs(msg->axes[angular_axis2_]) > std::abs(ang))
ang = msg->axes[angular_axis2_];
}
else
ROS_ERROR("Out of range: number of axis (%lu) must be greater than angular_axis2 (%d).",
msg->axes.size(), angular_axis2_);
}
float lin_x = getJoyValue(msg, linear_axis_, linear_axis2_, "linear_axis");
float lin_y = getJoyValue(msg, linear_y_axis_, linear_y_axis2_, "linear_y_axis");
float ang = getJoyValue(msg, angular_axis_, angular_axis2_, "angular_axis");

if (high_speed_button_ >= 0)
{
if (static_cast<size_t>(high_speed_button_) < msg->buttons.size())
{
if (msg->buttons[high_speed_button_])
{
lin *= linear_high_speed_ratio_;
lin_x *= linear_high_speed_ratio_;
lin_y *= linear_high_speed_ratio_;
ang *= angular_high_speed_ratio_;
}
}
Expand All @@ -133,8 +129,9 @@ class JoystickInterrupt
}

geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = lin * linear_vel_;
cmd_vel.linear.y = cmd_vel.linear.z = 0.0;
cmd_vel.linear.x = lin_x * linear_vel_;
cmd_vel.linear.y = lin_y * linear_y_vel_;
cmd_vel.linear.z = 0.0;
cmd_vel.angular.z = ang * angular_vel_;
cmd_vel.angular.x = cmd_vel.angular.y = 0.0;
pub_twist_.publish(cmd_vel);
Expand Down Expand Up @@ -182,6 +179,10 @@ class JoystickInterrupt
pnh_.param("linear_high_speed_ratio", linear_high_speed_ratio_, 1.3);
pnh_.param("angular_high_speed_ratio", angular_high_speed_ratio_, 1.1);
pnh_.param("timeout", timeout_, 0.5);
pnh_.param("linear_y_vel", linear_y_vel_, 0.0);
pnh_.param("linear_y_axis", linear_y_axis_, -1);
pnh_.param("linear_y_axis2", linear_y_axis2_, -1);

last_joy_msg_ = ros::Time(0);

if (interrupt_button_ < 0)
Expand Down
115 changes: 112 additions & 3 deletions joystick_interrupt/test/src/test_joystick_interrupt.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <string>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
Expand All @@ -50,12 +52,12 @@ class JoystickInterruptTest : public ::testing::Test
}

public:
JoystickInterruptTest()
explicit JoystickInterruptTest(const std::string& cmd_vel_topic = "cmd_vel")
: nh_()
{
pub_cmd_vel_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_input", 1);
pub_joy_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
sub_cmd_vel_ = nh_.subscribe("cmd_vel", 1, &JoystickInterruptTest::cbCmdVel, this);
sub_cmd_vel_ = nh_.subscribe(cmd_vel_topic, 1, &JoystickInterruptTest::cbCmdVel, this);

ros::Rate wait(10);
for (size_t i = 0; i < 100; ++i)
Expand Down Expand Up @@ -88,11 +90,13 @@ class JoystickInterruptTest : public ::testing::Test
joy.buttons.resize(2);
joy.buttons[0] = button;
joy.buttons[1] = high_speed;
joy.axes.resize(4);
joy.axes.resize(6);
joy.axes[0] = lin0;
joy.axes[1] = ang0;
joy.axes[2] = lin1;
joy.axes[3] = ang1;
joy.axes[4] = 0;
joy.axes[5] = 0;
pub_joy_.publish(joy);
}
};
Expand Down Expand Up @@ -248,6 +252,111 @@ TEST_F(JoystickInterruptTest, InterruptHighSpeed)
}
}

class JoystickInterruptOmniTest : public JoystickInterruptTest
{
public:
JoystickInterruptOmniTest()
: JoystickInterruptTest("cmd_vel_omni")
{
}
void publishJoy(
const int button,
const int high_speed,
const float lin0,
const float lin_y0,
const float ang0,
const float lin1,
const float lin_y1,
const float ang1)
{
sensor_msgs::Joy joy;
joy.header.stamp = ros::Time::now();
joy.buttons.resize(2);
joy.buttons[0] = button;
joy.buttons[1] = high_speed;
joy.axes.resize(6);
joy.axes[0] = lin0;
joy.axes[1] = ang0;
joy.axes[2] = lin1;
joy.axes[3] = ang1;
joy.axes[4] = lin_y0;
joy.axes[5] = lin_y1;
pub_joy_.publish(joy);
}
};

TEST_F(JoystickInterruptOmniTest, Interrupt)
{
ros::Duration(1.0).sleep();
ros::Rate rate(20);
for (size_t i = 0; i < 25; ++i)
{
publishCmdVel(0.1, 0.2);
if (i < 5)
publishJoy(0, 0, 0, 0, 0, 0, 0, 0);
else if (i < 10)
publishJoy(1, 0, 0.5, 0, 0, 0, 0, 0);
else if (i < 15)
publishJoy(1, 0, 0.5, 0, 0, -1, 0, 0);
else if (i < 20)
publishJoy(1, 0, 0, 0.3, 0, 0, 0, 0);
else if (i < 25)
publishJoy(1, 0, 0, 0.3, 0, 0, 1, 0);
else if (i < 30)
publishJoy(1, 0, 0, 0, -0.7, 0, 0, 0);
else
publishJoy(1, 0, 0, 0, -0.7, 0, 0, 1);

rate.sleep();
ros::spinOnce();
if (i < 3)
continue;
ASSERT_TRUE(static_cast<bool>(cmd_vel_));
if (i < 5)
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.1, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 0.2, 1e-3);
}
else if (i < 10)
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.5, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
}
else if (i < 15)
{
ASSERT_NEAR(cmd_vel_->linear.x, -1.0, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
}
else if (i < 20)
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.15, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
}
else if (i < 25)
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.5, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
}
else if (i < 30)
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, -0.7, 1e-3);
}
else
{
ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
ASSERT_NEAR(cmd_vel_->angular.z, 1.0, 1e-3);
}
}
}

class JoystickMuxTest : public ::testing::Test
{
protected:
Expand Down
16 changes: 16 additions & 0 deletions joystick_interrupt/test/test/joystick_interrupt_rostest.test
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,22 @@
<param name="linear_high_speed_ratio" value="2.0" type="double" />
<param name="angular_high_speed_ratio" value="2.0" type="double" />
</node>
<node pkg="joystick_interrupt" type="joystick_interrupt" name="joystick_interrupt_omni">
<param name="interrupt_button" value="0" type="int" />
<param name="high_speed_button" value="1" type="int" />
<param name="linear_axis" value="0" type="double" />
<param name="angular_axis" value="1" type="double" />
<param name="linear_axis2" value="2" type="int" />
<param name="angular_axis2" value="3" type="int" />
<param name="linear_vel" value="1.0" type="double" />
<param name="angular_vel" value="1.0" type="double" />
<param name="linear_high_speed_ratio" value="2.0" type="double" />
<param name="angular_high_speed_ratio" value="2.0" type="double" />
<param name="linear_y_vel" value="0.5" type="double" />
<param name="linear_y_axis" value="4" type="double" />
<param name="linear_y_axis2" value="5" type="int" />
<remap from="cmd_vel" to="cmd_vel_omni"/>
</node>
<node pkg="joystick_interrupt" type="joystick_mux" name="joystick_mux">
<param name="interrupt_button" value="0" type="int" />
</node>
Expand Down