Backup of joystick control using ROS(No. 12)

SIGVerse

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
- ROS command
- SIGVerse controller
Compiling
World file
Running the server
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.

↑

ROS command †

sig_command.cpp file:

#include <ros/ros.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include "std_msgs/String.h"
#include <Sigvers_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<Sigvers_Ros::Velocity>("mycommand_velocity", 1);
  mess_sub = nh_.subscribe("CommandMess",100,&CommandSig::chatterCallback,this);
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "Sigvers_Command_node");
  CommandSig Command_sig;
 ros::Rate loop_rate(10);
 ros::spin();
  return(0);
}
void CommandSig::chatterCallback (const std_msgs::String::ConstPtr& msg)
{
Sigvers_Ros::Velocity vel;
if(strcmp("go",msg->data.c_str())==0)
{
vel.angular = 3.0;
vel.linear = 3.0;
vel_pub_.publish(vel);
}
   else if(strcmp("back",msg->data.c_str())==0)
   {
vel.angular = -3.0;
vel.linear = -3.0;
vel_pub_.publish(vel);
   }
     else if(strcmp("left",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("right",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); 
                    }
else if(strcmp("square",msg->data.c_str())==0)
                    {
vel.angular = 3.0;
vel.linear = 3.0; 
vel_pub_.publish(vel);
sleep(1.8);
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);
vel.angular = 3.0;
vel.linear = 3.0; 
vel_pub_.publish(vel);
sleep(1.8);
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);
vel.angular = 3.0;
vel.linear = 3.0;
vel_pub_.publish(vel);
sleep(1.8);
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); 
vel.angular = 3.0;
vel.linear = 3.0;
vel_pub_.publish(vel);
sleep(1.8);
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);
vel.angular = 3.0;
vel.linear = 3.0;
vel_pub_.publish(vel);
sleep(1.8); 
vel.angular = 0.0;
vel.linear =  0.0;
vel_pub_.publish(vel);
                     }
else if(strcmp("cercle",msg->data.c_str())==0)  {
vel.angular = 2.5;
vel.linear =  2.2;
vel_pub_.publish(vel); 
     }
}
void CommandSig::CommandLoop()
{
  ros::Rate loop_rate(10);
while (ros::ok)
{
 ros::spinOnce();
 loop_rate.sleep();
}
  return;
}

↑

SIGVerse controller †

sig_app.cpp:

#include "Controller.h"
#include "Logger.h"
#include "ControllerEvent.h"
#include "ros/ros.h"
#include <sstream>
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <Sigvers_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:
 //シミュレーション開始時に一度だけ呼出される関数onInitの利用を宣言します
 void onInit(InitEvent &evt);
 double onAction(ActionEvent &evt);
 void onRecvMsg(RecvMsgEvent &evt);
 void poseCallback(const Sigvers_Ros::VelocityConstPtr& vel);
public:
 RobotObj *my;
 double velocity;
 int i;
 ros::Publisher chatter_pub;
 ros::Subscriber  pose_sub ;
 double direction1;
 double direction2;
int j;
double Weel_one;
double Weel_two;
};
void SendController::poseCallback(const Sigvers_Ros::VelocityConstPtr& vel)
{
   direction1 =  vel->linear;
   direction2 =  vel->angular;
   Weel_one =  vel->linear;
   Weel_two =  vel->angular;
  printf("Velocity 1!!!  %f\n",direction1);
  printf("Velocity 2!!!  %f\n",direction2);
}
void SendController::onInit(InitEvent &evt)
{
direction1 =0.0;
direction2 =0.0;
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, "Sigvers_controller_node");
ros::NodeHandle n;
velocity = 1;
chatter_pub = n.advertise<std_msgs::String>("CommandMess", 1);
pose_sub = n.subscribe<Sigvers_Ros::Velocity>("mycommand_velocity",  1,&SendController::poseCallback,this);
ros::Rate loop_rate(10);
}
double SendController::
onAction(ActionEvent &evt)
{
 my->setWheelVelocity(Weel_one,Weel_two);
 ros::spinOnce();
 return 1.0;
}
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;
}

↑

Compiling †

The following line should be included in CMakeLists.txt for both packages:

include_directories (PATH-TO-SIGVERSE-LIBRARIES/include/sigverse PATH-TO-SIGVERSE-LIBRARIES/include/sigverse/comm/controller )

Then go to the catkin_ws directory to compile the project:

$ cd ~/catking_make

$ catkin_make

↑

World file †

Click on the link to download the world file:

RobotInMove.xml

The world file should be placed in "~/catkin_ws/devel/lib", where the .so files are generated.

↑

Running the server †

To run the server you have to follow these steps:

Go to catkin_ws and execute the following command :
```
$ source ./devel/setup.bash
```

Run the ROS system:
```
$ roscore
```
run the sig_command node :
```
$ rosrun sig_ros sig_command 
```
Go to catkin_ws/devel/lib/libsig_ros and run the sigverse controller:
```
$ sigserver.sh -w ./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","forward");
	}
	if(JS->getButton(1))
	{
          this->sendMsg("robot_000","back");
	}
	if(JS->getButton(2))
	{
          this->sendMsg("robot_000","right");
	}
	if(JS->getButton(3))
	{
          this->sendMsg("robot_000","left");
	}
	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

Backup of joystick control using ROS (No. 12)