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 †

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 it, then publishes the command on the velocity topic, finally the SIGVerse command node receives the command and send it to the robot.

ros_Joyestick.PNG

↑

Creating projects †

Go to catkin_ws directory and create two new packages:

$ cd ~/catkin_ws/src
$ catkin_create_pkg key_sig
$ catkin_create_pkg  Sigvers_Ros
↑

Server †

In the server side, we have two controllers, the SIGVerse controller which creates a node that receives data from joystick and controls the agent, and the ROS command controller which creates another node that generates the command for the agent.

↑

SIGVerse controller †

sig_ros.cpp file:

#include "Controller.h"
#include "Logger.h"
#include "ControllerEvent.h"
#include "ros/ros.h"
#include <sstream>
#include <turtlesim/Velocity.h>
#include "std_msgs/String.h"
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.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 turtlesim::VelocityConstPtr& vel);
public:
 RobotObj *my;
 turtlesim::Velocity vel;
 double velocity;
 int i;
 ros::Publisher chatter_pub;
 ros::Subscriber  pose_sub ;
 double direction1;
 double direction2;
int j;
turtlesim::Pose gpo;
};
void SendController::poseCallback(const turtlesim::VelocityConstPtr& vel)
{
   direction1 =  vel->linear;
   direction2 =  vel->angular;
}
void SendController::onInit(InitEvent &evt)
{
direction1 =0.0;
direction2 =0.0;
int argc =0;
char** argv = NULL;
my = this->getRobotObj(this->myname());
my->setWheel(10.0, 10.0);
ros::init(argc, argv, "talker");
ros::NodeHandle n;
velocity = 1;
pose_sub = n.subscribe<turtlesim::Velocity>("mycommand_velocity",  1,&SendController::poseCallback,this);
 i =0.3;
ros::Rate loop_rate(100);
 j=0.3;
}
double SendController::onAction(ActionEvent &evt)
{
 std::string msg = "Hello!!";
Vector3d myPos;
my->getPosition(myPos);
if(direction1 < 0)
{
  my->setWheelVelocity(-velocity, -velocity);
}
 else if(direction1 >0)
{
my->setWheelVelocity(velocity, velocity);
}
if (direction2 < 0)
{
my->setWheelVelocity(-velocity/4, velocity/4);
}
 else if (direction2 > 0)
{
my->setWheelVelocity(velocity/4, -velocity/4);
}
if(direction1==0 && direction2 == 0)
{
my->setWheelVelocity(0.0,0.0);
 }
ros::spinOnce();
 return 1.0;
}
void SendController::onRecvMsg(RecvMsgEvent &evt)
{ 
}
 extern "C"  Controller * createController ()
 {
  return new SendController;
}
↑

ROS command †

Sig_command.cpp:

#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("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;
}
↑

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:

fileRobotInMove.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:

↑

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("go 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","go");
	}
	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("136.187.35.129", 9000);
	srv.startLoop();
	return 0;
 }
↑

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

Front page   New List of pages Search Recent changes   Help   RSS of recent changes