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.
Download and install Microsoft directx sdk
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.
Go to catkin_ws directory and create a new package:
$ cd ~/catkin_ws/src $ catkin_create_pkg sig_ros
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.
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: //シミュレーション開始時に一度だけ呼出される関数onInitの利用を宣言します 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 ; //ros::Publisher pose_pub ; double direction1; double direction2; int j; double Weel_one; double Weel_two; }; void SendController::poseCallback(const sig_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, "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(); //1秒おきにonActionが呼び出されます return 0.01; } 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_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; }
Click on the link to download the world file:
Open Visual studio and add new C++ project, then include sigverse libraries.
Joystick_Service.cpp
To download the plugin:
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:
To install and run the server, please refer to the SIGVerse/ROS integration? tutorial.