Gazebo——键盘控制

参考http://www.guyuehome.com/253

直接参考http://wiki.ros.org/teleop_twist_keyboard,官方的keyboard control文档,最方便。

1、创建控制包

  1. catkin_create_pkg keyboard_control rospy geometry_msgs std_msgs roscpp

    2、python控制

    在keyboard_control下创建文件夹scripts,创建python代码。

    #!/usr/bin/env python
    import roslib; roslib.load_manifest('keyboard_control')
    import rospy
    from geometry_msgs.msg import Twist
    from std_msgs.msg import String
    
    class Teleop:
     def __init__(self):
     rospy.init_node('keyboard_control')
     pub = rospy.Publisher('cmd_vel', Twist)
     rate = rospy.Rate(rospy.get_param('~hz', 1))
     self.cmd = None
    
     cmd = Twist()
     cmd.linear.x = 0.2
     cmd.linear.y = 0
     cmd.linear.z = 0
     cmd.angular.z = 0
     cmd.angular.z = 0
     cmd.angular.z = 0.5
    
     self.cmd = cmd
     while not rospy.is_shutdown():
     str = "hello world %s" % rospy.get_time()
     rospy.loginfo(str)
     pub.publish(self.cmd)
     rate.sleep()
    
    if __name__ == '__main__':Teleop()

    3、非常重要的一步

    sudo chmod +x keyboard_control.py

4、catkin_make

source devel/setup.bash

roscore

rosrun keyboard_control keyboard_control.py

5、建立launch文件。

在package下创建launch目录,然后创建keyboard_control.launch文件。

<launch>

<arg name=”cmd_topic” default=”cmd_vel” />

<node pkg=”smartcar_teleop” output=”screen” type=”teleop.py” name=”smartcar_teleop”>

<remap from=”cmd_vel” to=”$(arg cmd_topic)” />

</node>

</launch>

6、添加cpp文件

在src下面创建keyboard_control_cpp.cpp文件。

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>

#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64

#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57

class SmartCarKeyboardTeleopNode
{
private:
double walk_vel_;
double run_vel_;
double yaw_rate_;
double yaw_rate_run_;

geometry_msgs::Twist cmdvel_;
ros::NodeHandle n_;
ros::Publisher pub_;

public:
SmartCarKeyboardTeleopNode()
{
pub_ = n_.advertise<geometry_msgs::Twist>(“cmd_vel”, 1);

ros::NodeHandle n_private(“~”);
n_private.param(“walk_vel”, walk_vel_, 0.5);
n_private.param(“run_vel”, run_vel_, 1.0);
n_private.param(“yaw_rate”, yaw_rate_, 1.0);
n_private.param(“yaw_rate_run”, yaw_rate_run_, 1.5);
}

~SmartCarKeyboardTeleopNode() { }
void keyboardLoop();

void stopRobot()
{
cmdvel_.linear.x = 0.0;
cmdvel_.angular.z = 0.0;
pub_.publish(cmdvel_);
}
};

SmartCarKeyboardTeleopNode* tbk;
int kfd = 0;
struct termios cooked, raw;
bool done;

int main(int argc, char** argv)
{
ros::init(argc,argv,”tbk”, ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
SmartCarKeyboardTeleopNode tbk;

boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));

ros::spin();

t.interrupt();
t.join();
tbk.stopRobot();
tcsetattr(kfd, TCSANOW, &cooked);

return(0);
}

void SmartCarKeyboardTeleopNode::keyboardLoop()
{
char c;
double max_tv = walk_vel_;
double max_rv = yaw_rate_;
bool dirty = false;
int speed = 0;
int turn = 0;

// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);

puts(“Reading from keyboard”);
puts(“Use WASD keys to control the robot”);
puts(“Press Shift to move faster”);

struct pollfd ufd;
ufd.fd = kfd;
ufd.events = POLLIN;

for(;;)
{
boost::this_thread::interruption_point();

// get the next event from the keyboard
int num;

if ((num = poll(&ufd, 1, 250)) < 0)
{
perror(“poll():”);
return;
}
else if(num > 0)
{
if(read(kfd, &c, 1) < 0)
{
perror(“read():”);
return;
}
}
else
{
if (dirty == true)
{
stopRobot();
dirty = false;
}

continue;
}

switch(c)
{
case KEYCODE_W:
max_tv = walk_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S:
max_tv = walk_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A:
max_rv = yaw_rate_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D:
max_rv = yaw_rate_;
speed = 0;
turn = -1;
dirty = true;
break;

case KEYCODE_W_CAP:
max_tv = run_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S_CAP:
max_tv = run_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = -1;
dirty = true;
break;
default:
max_tv = walk_vel_;
max_rv = yaw_rate_;
speed = 0;
turn = 0;
dirty = false;
}
cmdvel_.linear.x = speed * max_tv;
cmdvel_.angular.z = turn * max_rv;
pub_.publish(cmdvel_);
}
}

7、变更CMakeLists.txt

添加下面的文件。

## Declare a C++ executable
add_executable(keyboard_control src/keyboard_control_cpp.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(keyboard_control boost_thread)

## Specify libraries to link a library or executable target against
target_link_libraries(keyboard_control
${catkin_LIBRARIES}
)

发布者

harifun

小学的时候,说自己要当一名科学家!那时候,看到新闻联播在宣传“四化”,立志要为现代化作出贡献!高中立志要做机器人,本科和硕士学的自动化!毕业进入华为,因为是测试岗位而离开,然后进入创业公司做工业扫地机器人和服务式机器人!A轮完成数千万级融资后因家庭原因离开上海,回武汉进入一家上市公司从事激光雷达设计工作!机器人是我一直以来的追求,希望有一天我能实现我的理想!

发表评论

电子邮件地址不会被公开。 必填项已用*标注