ROS x JoyStick で遊んでみる
やりたいこと
rosでjoy stickを操作してみる
教科書
ライセンス
<maintainer email="fei@kanto-gakuin.ac.jp">Fei Qian</maintainer> <license>BSD</license> <author>Fei Qian</author>
設定手順
デバイス確認
- ls /dev/input/js*
命令の確認
- jstest /dev/input/js0
パッケージのインストール
sudo apt-get install ros-kinetic-joystick-drivers
入力デバイスの変更
- rosparam set joy_node/dev "dev/input/jsX" <- X:デバイス番号
実行手順
rosrun joy joy_node
rostopic list
/diagnostics /joy /rosout /rosout_agg
- rostopic info /joy
Type: sensor_msgs/Joy Publishers: * /joy_node (http://172.17.0.1:34890/) Subscribers: None
- rosmsg show sensor_msgs/Joy
std_msgs/Header header uint32 seq time stamp string frame_id float32[] axes int32[] buttons
- rostopic echo joy
header:
seq: 1
stamp:
secs: 1516022983
nsecs: 706817308
frame_id: ''
axes: [0.0, 1.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 0]
---
header:
seq: 2
プログラム作成
子ガメをjoy stickで操作してみる
rosrun turtlesim turtlesim_node &
rosnode info /turtlesim
Node [/turtlesim]
Publications:
* /rosout [rosgraph_msgs/Log]
* /turtle1/color_sensor [turtlesim/Color]
* /turtle1/pose [turtlesim/Pose]
Subscriptions:
* /turtle1/cmd_vel [unknown type]
Services:
* /clear
* /kill
* /reset
* /spawn
* /turtle1/set_pen
* /turtle1/teleport_absolute
* /turtle1/teleport_relative
* /turtlesim/get_loggers
* /turtlesim/set_logger_level
contacting node http://172.17.0.1:38981/ ...
Pid: 6614
Connections:
* topic: /rosout
* to: /rosout
* direction: outbound
* transport: TCPROS
- rostopic type /turtle1/cmd_vel
geometry_msgs/Twist
- rosmsg show -r geometry_msgs/Twist
# This expresses velocity in free space broken into its linear and angular parts. Vector3 linear Vector3 angular
- rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z
実装
フォルダ構成
├── CMakeLists.txt
└── my_joystick
├── CMakeLists.txt
├── launch
│ └── my_joystick.launch
├── package.xml
└── src
└── my_joystick.cpp
my_joystick.cpp
Aボタンで速直進
Bボタンで速回転
#include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <sensor_msgs/Joy.h> // class definition class TeleopTurtle { public: //constructor TeleopTurtle(); //hiding private: void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); ros::NodeHandle nh; int vel_linear, vel_angular; int lin_boost, ang_boost; double l_scale_, a_scale_; ros::Publisher vel_pub_; ros::Subscriber joy_sub_; }; //constructor TeleopTurtle::TeleopTurtle(): vel_linear(1), vel_angular(0), a_scale_(3.2),l_scale_(2.0), lin_boost(0), ang_boost(1) //initialize { nh.param("axis_linear" , vel_linear , vel_linear); //set parameter nh.param("axis_angular" , vel_angular, vel_angular); nh.param("angular_boost" , ang_boost, ang_boost); nh.param("linear_boost" , lin_boost, lin_boost); nh.param("scale_angular", a_scale_, a_scale_); nh.param("scale_linear" , l_scale_, l_scale_); //subsucriber,use template //this pointa is object pointa joy_sub_ = nh.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::joyCallback, this); vel_pub_ = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1); } void TeleopTurtle::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) { geometry_msgs::Twist twist; if (joy->buttons[ang_boost] > 0){ twist.angular.z = 2 * a_scale_*joy->axes[vel_angular]; }else{ twist.angular.z = a_scale_*joy->axes[vel_angular]; } if (joy->buttons[lin_boost] > 0){ twist.linear.x = 2 * l_scale_*joy->axes[vel_linear]; }else{ twist.linear.x = l_scale_*joy->axes[vel_linear]; } ROS_INFO_STREAM("(" << joy->axes[vel_linear] << " " << joy->axes[vel_angular] << ")"); vel_pub_.publish(twist); } int main(int argc, char** argv) { ros::init(argc, argv, "teleop_turtle"); TeleopTurtle teleop_turtle; ros::spin(); return 0; }
launch
<launch> <!-- Turtlesim Node --> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <!-- joy node --> <node pkg="joy" type="joy_node" name="teleopJoy" output="screen"> <param name="dev" type="string" value="/dev/input/js0" /> <param name="deadzone" value="0.12" /> <param name="autorepeat_rate" value="1.0" /> <param name="coalesce_interval" value="0.0" /> </node> <!-- define buttons & Axes for fami con Pad--> <param name="axis_linear" value="1" type="int" /> <param name="axis_angular" value="0" type="int" /> <param name="linear_boost" value="0" type="int" /> <param name="angular_boost" value="1" type="int" /> <param name="scale_linear" value="3.0" type="double" /> <param name="scale_angular" value="2.0" type="double" /> <node pkg="my_joystick" type="my_joystick" name="my_joystick" launch-prefix="xterm -font r14 -bg darkblue -geometry 113x30+503+80 -e" output="screen" required="true" /> </launch>
CMakeList.txt
cmake_minimum_required(VERSION 2.8.3)
project(my_joystick)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
sensor_msgs
std_msgs
)
catkin_package(
CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(my_joystick src/my_joystick.cpp)
target_link_libraries(my_joystick
${catkin_LIBRARIES}
)
package.xml
<buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>roscpp</build_depend> <build_depend>sensor_msgs</build_depend> <build_depend>std_msgs</build_depend> <build_depend>turtlesim</build_depend> <build_depend>sensor_msgs</build_depend> <build_depend>joy</build_depend> <run_depend>geometry_msgs</run_depend> <run_depend>roscpp</run_depend> <run_depend>sensor_msgs</run_depend> <run_depend>std_msgs</run_depend> <run_depend>turtlesim</run_depend> <run_depend>sensor_msgs</run_depend> <run_depend>joy</run_depend>
