My teleop node is not very reactive
I wrote my own teleop node in order to use my joypad with my robot. It works fine, but the robot takes few addition seconds to stop even if I already released the stick on the joypad.
It seems a buffer problem or something related to the node frame rate, but I'm not so sure.
This is my teleop node:
TeleopRobo::TeleopRobo():
linear_(1),
angular_(2)
{
nh_.param("axis_linear", linear_, linear_);
nh_.param("axis_angular", angular_, angular_);
nh_.param("axis_rotation", rotation_, rotation_);
nh_.param("scale_angular", a_scale_, a_scale_);
nh_.param("scale_linear", l_scale_, l_scale_);
nh_.param("scale_rotation", s_rotation_, s_rotation_);
vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 1000, &TeleopRobo::joyCallback, this);
}
void TeleopRobo::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
geometry_msgs::Twist twist;
twist.linear.y = a_scale_*joy->axes[angular_];
twist.linear.x = l_scale_*joy->axes[linear_];
twist.angular.z = -s_rotation_*joy->axes[rotation_];
vel_pub_.publish(twist);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "teleop_robo");
TeleopRobo teleop_robo;
ros::Rate loop_rate(10);
ros::spin();
}
and this is the node used to control my robot:
int main(int argc, char **argv)
{
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::Rate loop_rate(10);
ros::Subscriber cmd_vel = n.subscribe("cmd_vel", 1000, &RoboteqDevice::cmd_velCallback, &device);
ros::spin();
return 0;
}
void RoboteqDevice::cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
ROS_INFO("X=%.3f Y= %.3f Z=%.3f Angle: %.1f ", (float) msg->linear.x, (float) msg->linear.y, (float) msg->linear.z, (float) msg->angular.z);
vel_x = msg->linear.x;
vel_y = msg->linear.y;
vteta_z = msg->angular.z;
// send commands to motors
}
This is my rqt_graph output:
If I use rostopic echo /cmd_vel
I can see that the values instantly go to zero when I release the stick, so the problem should be on the base_controller node.
What do you think?
EDIT1:
I changed the queue size to 1 in the subscriber and now the robot is very reactive! Thank you!
The problem is that the robot stops after a while even if I'm still pushing the stick. I checked the output of /joy
topic and when the robot stop also the output of the /joy
topic stops. Then, I need to release the stick or to press another button in order to restore the joystick. Why this happen?