joystick control using ROS

Up:Tutorial?

The aim of this tutorial is to show how to control a SIGVerse agent using Joystick. ROS operating system is used to process data coming from the joystick and send them to the SIGVerse controller.

Installation of the joystick
Overview
Creating the project
Server
- SIGVerse controller
- ROS command
World file
Service
Download the project

Installation of the joystick †

Download and install Microsoft directx sdk

↑

Overview †

When the server is launched, 2 ROS nodes are created, the SIGVerse controller node and SIGVerse command node, this involves the creation of 2 topics, command message topic and velocity topic. In the other part, the joystick service sends data to the SIGVerse command node which will publish it on the command message topic, then, the SIGVerse controller node receives the data, processes them, then publishes the corresponding command on the velocity topic, finally the SIGVerse command node receives the command and sends it to the robot.

↑

Creating the project †

Go to catkin_ws directory and create a new package:

$ cd ~/catkin_ws/src
$ catkin_create_pkg sig_ros

↑

Server †

In the server side we have two nodes, SIGVerwe controller node and ROS command node, the first one aims to receive data from the joystick, send them to the second node then receive the command, the second node receives the data from the first node and send the corresponding command.

↑

SIGVerse controller †

sig_controller.cpp:

#include "Controller.h"
#include "Logger.h"
#include "ControllerEvent.h"
#include "ros/ros.h"
#include <sstream>
#include "std_msgs/String.h"
#include <sig_ros/Velocity.h>
#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )
#define ARY_SIZE(ARY) ( (int)(sizeof(ARY)/sizeof(ARY[0])) )
class SendController : public Controller
{
  public:
   void onInit(InitEvent &evt);
   double onAction(ActionEvent &evt);
   void onRecvMsg(RecvMsgEvent &evt);
   void poseCallback(const sig_ros::VelocityConstPtr& vel);
  public:
   RobotObj *my;
   double velocity;
   int i;
   ros::Publisher chatter_pub;
   ros::Subscriber  pose_sub ;
   int j;
   double Weel_one;
   double Weel_two;
};
 
void SendController::poseCallback(const sig_ros::VelocityConstPtr& vel)
{
   Weel_one =  vel->linear;
   Weel_two =  vel->angular;
}

void SendController::onInit(InitEvent &evt)
{
  Weel_one = 0.0;
  Weel_two = 0.0;
  int argc =0;
  char** argv = NULL;
  my = this->getRobotObj(this->myname());
  my->setWheel(10.0, 10.0);
  ros::init(argc, argv, "sig_controller_node");
  ros::NodeHandle n;
  velocity = 1;
  chatter_pub = n.advertise<std_msgs::String>("message_command", 1);
  pose_sub = n.subscribe<sig_ros::Velocity>("mycommand_velocity", 1,&SendController::poseCallback,this);
  ros::Rate loop_rate(10);
  // Running the ros_command node
  system ("source ~/catkin_ws/devel/setup.sh");
  system ("rosrun sig_ros ros_controller &");
}

double SendController::onAction(ActionEvent &evt)
{
  my->setWheelVelocity(Weel_one,Weel_two);
  ros::spinOnce();
  return 0.01; //onAction will be called every 0.01second
}

void SendController::onRecvMsg(RecvMsgEvent &evt)
{
  std::string sender = evt.getSender();
  std::string msg = evt.getMsg();
  LOG_MSG(("message : %s", msg.c_str()));
  std_msgs::String mesg;
  std::stringstream ss;
  ss << msg;
  mesg.data = ss.str();
  chatter_pub.publish(mesg);
}
extern "C"  Controller * createController ()
{
  return new SendController;
}

↑

ROS command †

ros_controller.cpp file:

#include <ros/ros.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include "std_msgs/String.h"
#include <sig_ros/Velocity.h>
#define KEYCODE_R 0x43
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71
class CommandSig
{
 public:
   CommandSig();
   void CommandLoop();
   void chatterCallback (const std_msgs::String::ConstPtr& msg);
 private:
   ros::NodeHandle nh_;
   double linear_, angular_, l_scale_, a_scale_;
   ros::Publisher vel_pub_;
   ros::Subscriber  mess_sub ;
};

CommandSig::CommandSig():
   linear_(0),
   angular_(0),
   l_scale_(2.0),
   a_scale_(2.0)
{
   nh_.param("scale_angular", a_scale_, a_scale_);
   nh_.param("scale_linear", l_scale_, l_scale_);
   vel_pub_ = nh_.advertise<sig_ros::Velocity>("mycommand_velocity", 1);
   mess_sub =  nh_.subscribe("message_command",100,&CommandSig::chatterCallback,this);
}

int main(int argc, char** argv)
{
   ros::init(argc, argv, "ros_command_node");
   CommandSig Command_sig;
   ros::Rate loop_rate(100);
   ros::spin();
   return(0);
}

void CommandSig::chatterCallback (const std_msgs::String::ConstPtr& msg)
{
  sig_ros::Velocity vel;
  if(strcmp("moveforward",msg->data.c_str())==0)
    {
      vel.angular = 3.0;
      vel.linear = 3.0;
      vel_pub_.publish(vel);
    }
  else if(strcmp("movebackward",msg->data.c_str())==0)
    {
      vel.angular = -3.0;
      vel.linear = -3.0;
      vel_pub_.publish(vel);
    }
  else if(strcmp("turnleft",msg->data.c_str())==0)
    {
      vel.angular = 0.78;
      vel.linear = -0.78;
      vel_pub_.publish(vel);
      sleep(1.8);
      vel.angular = 0.0;
      vel.linear =  0.0;
      vel_pub_.publish(vel);
    }
  else if(strcmp("turnright",msg->data.c_str())==0)
    {
      vel.angular = -0.78;
      vel.linear = 0.78;
      vel_pub_.publish(vel);
      sleep(1.8);
      vel.angular = 0.0;
      vel.linear =  0.0;
      vel_pub_.publish(vel);
    }
  else if(strcmp("stop",msg->data.c_str())==0)
    {
      vel.angular = 0.0;
      vel.linear = 0.0;
      vel_pub_.publish(vel); 
    }
}

void CommandSig::CommandLoop()
{
  ros::Rate loop_rate(10);
  while (ros::ok)
    {
      ros::spinOnce();
      loop_rate.sleep();
    }
  return;
}

↑

World file †

Click on the link to download the world file:

RobotInMove.xml

↑

Service †

Open Visual studio and add new C++ project, then include sigverse libraries.

Joystick_Service.cpp

#include <tchar.h>
#include <string>
#include <iostream>
#include < fstream>
#include "SIGService.h"
#include <windows.h>
#include "Joystick_Driver.h"
bool Proceed = false;
bool lancer = false;
class Joystick_Service : public sigverse::SIGService
  {
public:
	Joystick_Service(std::string name);
	~Joystick_Service();
    double onAction();
	void onRecvMsg(sigverse::RecvMsgEvent &evt);
private: Joystick*  JS; //pointer to joystick object
private: HWND hWnd;	
 };
  Joystick_Service::Joystick_Service(std::string name) : SIGService(name){
            JS = new Joystick();
            hWnd = NULL;
	};
Joystick_Service::~Joystick_Service()
 {
	this->disconnect();
 }
double Joystick_Service::onAction()
 {
    JS->runJoystick(hWnd);
	printf("forward message        : %d\n",JS->getButton(0));
	printf("back message      : %d\n",JS->getButton(1));
	printf("left message      : %d\n",JS->getButton(2));
	printf("right message     : %d\n",JS->getButton(3));
	if(JS->getButton(0))
	{
          this->sendMsg("robot_000","moveforward");
	}
	if(JS->getButton(1))
	{
          this->sendMsg("robot_000","movebackward");
	}
	if(JS->getButton(2))
	{
          this->sendMsg("robot_000","turnright");
	}
	if(JS->getButton(3))
	{
          this->sendMsg("robot_000","turnleft");
	}
	return 0.1;
}
    void Joystick_Service::onRecvMsg(sigverse::RecvMsgEvent &evt)
   {
}
   int main(int argc, char** argv)
{	Joystick_Service srv("Joystick__Service");
	srv.connect(argv[1], port);
	srv.startLoop();
	return 0;
}

To download the plugin:

Sig_Joystick.sig

↑

Download the project †

To download the project from GIT repository, use the following link:

git@socio4.iir.nii.ac.jp:~/SigverseGitServer/unstable/usersContribution/tafifet/ros_joystick.git

The project contains:

"Sig_Joystick" directory: which is the joystick service, it is a VS 2008 project, it must be run in the client side.
"sig_ros" directory: which is the controller, it must be copied in the server side in "catkin_ws/src" directory.
"RobotMove.xml" file: the world file, it must be placed where the library is generated.

To install and run the server, please refer to the ROS integration tutorial.

Tutorial?