![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
The aim of this tutorial is to show how to control a SIGVerse agent using Joystick. ROS operating system is used to process data from the joystick and send them to the SIGVerse controller.
Download and install Microsoft directx sdk
When the controller is launched, 2 ROS nodes are created, the SIGVerse command node and SIGVerse controller 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.
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;
}
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;
}
#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;
}
To download the project from GIT depository, use the following link:
git@socio4.iir.nii.ac.jp:~/SigverseGitServer/unstable/usersContribution/tafifet/ros_joystick.git
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.