Raspberry_PI_Robot_Demo/joy_ws
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化手柄控制（解决在不操作手柄时出现溜车问题

Browse Source
This commit is contained in:
Ubuntu 2024-03-18 15:00:49 +08:00
parent 7cf3a252f7
commit ecc1df53c6
2 changed files with 25 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/huanyu_joy/include/huanyu_joy/huanyu_joy.h
View File

@ -42,6 +42,7 @@ private:
ros::NodeHandle n;
ros::Publisher cmd_vel_pub;
geometry_msgs::Twist vel_msg_;
bool publicTopic;
public:
huanyu_joy();

31
src/huanyu_joy/src/huanyu_joy.cpp
View File

@ -1,5 +1,5 @@
#include "huanyu_joy.h"
#include <iostream>
huanyu_joy::huanyu_joy()
@ -7,6 +7,7 @@ huanyu_joy::huanyu_joy()
joystick_fd = -1;
leftVertical = rightVertical = 0;
rightHorizontal = leftHorizontal = 0;
publicTopic = true;
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("joystick_device", joystick_device, "/dev/input/js0");
@ -18,9 +19,12 @@ huanyu_joy::huanyu_joy()
cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1000);
joystick_fd = open(joystick_device.c_str(), O_RDONLY | O_NONBLOCK); /* read write for force feedback? */
if (joystick_fd < 0)
if (joystick_fd > 0)
{
ROS_INFO("Open joystick device success!");
}else
{
ROS_ERROR("Open joystick device failed!");
}
}
@ -32,7 +36,7 @@ huanyu_joy::huanyu_joy()
void huanyu_joy::publish_joystick_event()
{
ros::Rate rosSleep(20);
ros::Rate rosSleep(100);
while(ros::ok())
{
int bytes = read(joystick_fd, &jse, sizeof(jse));
@ -55,6 +59,8 @@ void huanyu_joy::publish_joystick_event()
}
if (jse.type == JS_EVENT_BUTTON && jse.value == JS_EVENT_BUTTON_DOWN)
{
ROS_INFO("[%d %d]", jse.type, jse.value);
ROS_INFO("%d", jse.number);
switch(jse.number)
{
case 4:maxLinear_x += 0.1;
@ -66,17 +72,28 @@ void huanyu_joy::publish_joystick_event()
break;
case 7:(maxAngular_z > 0.1)?(maxAngular_z -= 0.1):(maxAngular_z = maxAngular_z);
break;
case 8: publicTopic = !publicTopic;
if(publicTopic)
{ROS_INFO("Open publish cmd_vel!");}
else
{ROS_INFO("Close publish cmd_vel!");}
break;
default: break;
}
ROS_INFO("maxLinear_x,maxAngular_z = [%f %f]",maxLinear_x, maxAngular_z);
if(jse.number != 8 )
{
ROS_INFO("maxX = [%.2f], maxY = [%.2f], maxZ = [%.2f]",maxLinear_x, maxLinear_y, maxAngular_z);
}
}
memset(&jse, 0, sizeof(jse));
if (publicTopic)
{
vel_msg_.linear.y = maxLinear_y /32767 * leftHorizontal * -1;
vel_msg_.linear.x = maxLinear_x /32767 * leftVertical * -1;
vel_msg_.angular.z = maxAngular_z/32767 * rightHorizontal * -1;
cmd_vel_pub.publish(vel_msg_);
}
rosSleep.sleep();
ros::spinOnce();
}
@ -86,7 +103,7 @@ void huanyu_joy::publish_joystick_event()
int main(int argc, char *argv[])
{
ros::init(argc, argv, "joy_controller");
ROS_INFO("[ZHOUXUEWEI] joy controller node start! ");
ROS_INFO("joy controller node start...... ");
huanyu_joy huanyu_joystick;
huanyu_joystick.publish_joystick_event();