ros pythonコードをc++コードへ書き換えたい
解決したいこと
PythonをC++に書き換えたいのですが上手くいかなく，助けてほしいです．
株式会社アールティのラズパイマウスROSV3を使って，移動ロボットの制御を行いたく，C++で書いているのですが上手くいかないです．詳しい方がいましたらアドバイスを頂きたいです．
書き換えたいPythonのコード
light_sensor.py
import sys, rospy
from mymouse_ros.msg import LightSensorValues
def get_freq():
f = rospy.get_param('lightsensors_freq',10)
try:
if f <= 0.0:
raise Exception()
except:
rospy.logerr('value error: lightsensors_frew')
sys.exit(1)
return f
if __name__ == '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('lightsensors')
pub = rospy.Publisher('light_vals', LightSensorValues, queue_size=1)
freq = get_freq()
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
try:
with open(devfile,'r') as f:
data = f. readline().split()
data = [ int(e) for e in data ]
d = LightSensorValues()
d.front_right = data[0]
d.right = data[1]
d.left = data[2]
d.front_left = data[3]
d.sum_all = sum(data)
d.front_average = int((data[0]+data[3])/2)
pub.publish(d)
except IOError:
rospy.logerr("can't write to "+ devfile)
f = get_freq()
if f != freq:
freq = f
rate = rospy.Rate(freq)
rate.sleep()
motors.py
import sys, rospy, math
from mymouse_ros.msg import MotorFreqs
from mymouse_ros.srv import TimedMotion #サービス用メッセージ型をインポート
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse #サービス用メッセージ型をインポート
class Motor():
def __init__(self):
#起動直後はソフトスイッチをoffにする（できなければ終了）
if not self.set_power(False): sys.exit(1)
rospy.on_shutdown(self.set_power)
self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)
#サービスの生成
self.srv_on = rospy.Service('motor_on', Trigger, self.callback_on)
self.srv_off = rospy.Service('motor_off', Trigger, self.callback_off)
self.srv_tm = rospy.Service('timed_motion', TimedMotion, self.callback_tm)
self.last_time = rospy.Time.now()
self.using_cmd_vel = False
def set_power(self, onoff = False): #onoffのデフォルトをFalse(OFF)
en = '/dev/rtmotoren0'
try:
with open(en, 'w') as f:
f.write('1\n' if onoff else '0\n')
self.is_on = onoff
return True
except:
rospy.logerr('cannot write to ' + en)
return False
def set_raw_freq(self, left_hz, right_hz):
if not self.is_on:
rospy.logerr('not enpowered')
return
try:
with open('/dev/rtmotor_raw_l0', 'w') as lf, \
open('/dev/rtmotor_raw_r0', 'w') as rf:
lf.write(str(int(round(left_hz))) + '\n')
rf.write(str(int(round(right_hz))) + '\n')
except:
rospy.logerr('cannot write to rtmotor_raw_*')
def callback_raw_freq(self, message):
self.set_raw_freq(message.left_hz, message.right_hz)
def callback_cmd_vel(self, message):
forward_hz = 80000.0*message.linear.x/(9*math.pi)
rot_hz = 400.0*message.angular.z/math.pi
self.set_raw_freq(forward_hz-rot_hz, forward_hz+rot_hz)
self.using_cmd_vel = True
self.last_time = rospy.Time.now()
#motor_on,motor_offの実体（といってもすでにset_powerがあるのでそれを使う）
def onoff_response(self, onoff):
d = TriggerResponse()
d.success = self.set_power(onoff)
d.message = "ON" if self.is_on else "OFF"
return d
#motor_onのコールバック関数（実際にはonoff_responseをTrueで呼び出し）
def callback_on(self, message):
return self.onoff_response(True)
#motor_offのコールバック関数（実際にはonoff_responseをFalseで呼び出し）
def callback_off(self, message):
return self.onoff_response(False)
#timed_motionのコールバック関数（実際には/dev/rtmotor0にleft_hz,right_hz,duration_msを書き込むだけ
def callback_tm(self, message):
if not self.is_on:
rospy.logerr("not enpowered")
return False
dev = '/dev/rtmotor0'
try:
with open(dev, 'w') as f:
f.write('%d %d %d\n' % (message.left_hz, message.right_hz, message.duration_ms))
except:
rospy.logerr('cannot write to ' + dev)
return False
return True
if __name__ == '__main__':
rospy.init_node('motors')
m = Motor()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#cmd_velで動作させた場合は1秒後に停止するようにしたが，本当は/dev/rtmotor0を使うべき
if m.using_cmd_vel and rospy.Time.now().to_sec() - m.last_time.to_sec() >= 1.0:
m.set_raw_freq(0,0)
m.using_cmd_vel = False
rate.sleep()
wall_trace.py
import rospy, copy
import math
from geometry_msgs.msg import Twist
from std_msgs.msg import UInt16
from std_srvs.srv import Trigger, TriggerResponse
from mymouse_ros.msg import LightSensorValues
from time import sleep
class WallStop():
def __init__(self):
# motorsノードで実装した/cmd_velをパブリッシャとして設定
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sensor_values = LightSensorValues()
# /lightsensorsノードの/light_valsをサブスクライバとして設定
rospy.Subscriber('/light_vals', LightSensorValues, self.callback)
# /buzzerノードの/write_freqをパブリッシャとして設定
self.write_freq = rospy.Publisher('/write_freq', UInt16, queue_size=1)
def callback(self, messages):
self.sensor_values = messages
# ロボットを動かす関数
def run(self):
rate = rospy.Rate(10)
data = Twist()
freq = UInt16
a = 0
while not rospy.is_shutdown():
data.linear.x = 0.2
data.angular.z = 0
freq = 0
# 壁にあたるまで直進
if self.sensor_values.sum_all < 300:
data.linear.x = 0.2
data.angular.z = 0
# 壁にあたると時計回りに90度
if self.sensor_values.sum_all >= 300 and a == 0:
data.linear.x = 0.2
data.angular.z = -math.pi*4/5
sleep(0.1)
freq = 0
a = 1
# 右に旋回
if self.sensor_values.left > 200 and a == 1:
data.linear.x = 0
data.angular.z = -math.pi*2/3
# 左に旋回
if self.sensor_values.left < 50 and a == 1:
data.linear.x = 0
data.angular.z = math.pi*2/3
# 角に来た時に少し直進してからマガル
if self.sensor_values.left < 50 and self.sensor_values.front_left < 80 and a == 1:
data.linear.x = 0.2
sleep(0.3)
data.angular.z = math.pi/2.3
# 上で得たdataをcmd_velにパブリッシュ
self.cmd_vel.publish(data)
self.write_freq.publish(freq)
rate.sleep()
if __name__ == '__main__':
rospy.init_node('wall_trace')
# 以下のwait_for_serviceはサービスが利用可能になる（motors.pyでサーバが立ち上がる）まで待つ関数
rospy.wait_for_service('/motor_on')
rospy.wait_for_service('/motor_off')
# /motor_off用のローカル関数を設定
off = rospy.ServiceProxy('/motor_off', Trigger)
# Ctrl+Cなど，ROSが終了するとき(on_shutdown)，安全のためoff関数（/motor_offサービス）を要求
rospy.on_shutdown(off)
# /motor_onサービスを要求し，動作準備
rospy.ServiceProxy('/motor_on', Trigger).call()
# ロボットの移動開始
WallStop().run()
C++コード（自分なりに書いてみたもの）
lightsensor.cpp
#include <iostream>
#include <sstream>
#include <vector>
#include <string>
#include <cstdlib>
int main (int argc, char **argv)
{
ros::init(argc, argv, "light_sensor");
ros::NodeHandle n;
ros::Publisher value_trans =
n.advertise<beginner_tutorials::LightSensorValues>("SensorValues",1000);
double freq =1;
n.getParam("light_sensor_freq",freq);
if(freq<0){freq = 1;}
ros::Rate loop_rate(freq);
std::vector<std::string> str_vec;
while(ros::ok())
{
std::cout<<"freq="<<freq<<std::endl;
std::fstream light_file;
std::string str;
light_file.open("/dev/rtlightsensor0");
getline(light_file, str);
int space_pos = 0;
for(int data_pos=0;data_pos<str.size();data_pos++)
{
if(str[data_pos]==' ')
{
str_vec.push_back(str.substr(space_pos, data_pos - space_pos));
space_pos = data_pos+1;
}
}
str_vec.push_back(str.substr(space_pos, str.size())); //last sesor value
beginner_tutorials::LightSensorValues lsv;
lsv.right_forward = std::atoi(str_vec[0].c_str());
lsv.right_side = std::atoi(str_vec[1].c_str());
lsv.left_side = std::atoi(str_vec[2].c_str());
lsv.left_forward = std::atoi(str_vec[3].c_str());
lsv.front_average= lsv.right_forward + lsv.left_forward;
lsv.sum_all = lsv.right_forward + lsv.right_side + lsv.left_side + lsv.left_forward;
str_vec.clear();
value_trans.publish(lsv);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Motor.h
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Int16.h"
#include "std_srvs/Trigger.h"
#include "beginner_tutorials/MotorFreqs.h"
#include "beginner_tutorials/TimedMotion.h"
#include "geometry_msgs/Twist.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#define QUEUE_SIZE 100
class Motor
{
public:
bool is_on;
bool using_cmd_vel;
ros::NodeHandle nh;
ros::Subscriber sub_raw;
ros::Subscriber sub_cmd_vel;
ros::Time last_time;
ros::ServiceServer srv_on;
ros::ServiceServer srv_off;
ros::ServiceServer srv_tm;
bool set_power(const bool motor_en)
{
std::ofstream motor_en_file;
try
{motor_en_file.open("/dev/rtmotoren0");}
catch(...)
{
std::cout << "motor switch ero" << std::endl;
return false;
}
motor_en_file << motor_en << std::endl;
motor_en_file.close();
is_on = motor_en;
return true;
}
void set_raw_freq(const int left_wheel, const int right_wheel)
{
if(!is_on)
{
std::cout << "motor power is off" <<std::endl;
return;
}
std::ofstream mt_file;
mt_file.open("/dev/rtmotor_raw_l0");
mt_file << left_wheel << std::endl;
mt_file.close();
mt_file.open("/dev/rtmotor_raw_ro");
mt_file << "right_wheel" << std::endl;
mt_file.close();
}
void callback_raw_message(const beginner_tutorials::MotorFreqs freq_msg)
{set_raw_freq(freq_msg.left,freq_msg.right);}
void callback_cmd_vel(const geometry_msgs::Twist twist_msg)
{
int forward_hz = 80000.0*twist_msg.linear.x/(9*M_PI);
int rot_hz = 4000.0*twist_msg.angular.z/M_PI;
set_raw_freq(forward_hz - rot_hz, forward_hz + rot_hz);
using_cmd_vel = true;
last_time = ros::Time::now();
}
std_srvs::Trigger::Response onoff_response(const bool onoff)
{
std_srvs::Trigger::Response d;
d.success = set_power(onoff);
d.message = is_on? "ON" : "OFF";
return d;
}
bool callback_on(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& resp)
{
resp = onoff_response(true);
return resp.success;
}
bool callback_off(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& resp)
{
resp = onoff_response(false);
return resp.success;
}
bool callback_tm(beginner_tutorials::TimedMotion::Request& req,
beginner_tutorials::timedMotion::Response& resp)
{
if(!is_on)
{
ROS_INFO("not enpowered");
return false;
}
std::ofstream motor_file;
motor_file.open("/dev/rtmotor0");
motor_file << req.left_hz << " " << req.right_hz << " " << req.duration_ms;
return true;
}
Motor()
{
if(set_power(true)){is_on = true;}
else{is_on = false;}
sub_raw = nh.subscribe("motor_raw" , QUEUE_SIZE,
&Motor::callback_raw_message,this);
sub_cmd_vel = nh.subscribe("cmd_vel",QUEUE_SIZE,
&Motor::callback_cmd_vel,this);
last_time = ros::Time::now();
using_cmd_vel = false;
srv_on = nh.advertiseService("motor_on",&Motor::callback_on,this);
srv_off = nh.advertiseService("motor_off",&Motor::callback_off,this);
srv_tm = nh.advertiseService("timed_motion", &Motor::callback_tm,this);
}
~Motor(){}
};
motors.cpp
#include "../include/beginner_tutorials/Motor.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "motor_freq");
ros::NodeHandle nh;
ros::Publisher pub_motor_raw;
Motor m;
pub_motor_raw = nh.advertise<beginner_tutorials::MotorFreqs>("motor_raw",1000);
ros::Rate rate(0.5);
beginner_tutorials::MotorFreqs msg_mf;
while(ros::ok()){
if(ros::Time::now().toSec() - m.last_time.toSec() > 10.0){
ROS_INFO("motor is shutting down");
msg_mf.left = 0;
msg_mf.right = 0;
pub_motor_raw.publish(msg_mf);
ros::spinOnce();
rate.sleep();
}
}
return 0;
}
wall_trace.cpp
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/UInt16.h"
#include "std_srvs/Trigger.h"
#include "beginner_tutorials/LightSensorValues.h"
#include "../include/beginner_tutorials/Motor.h"
#include <time.h>
#include <cmath>
#include <unistd.h>
beginner_tutorials::LightSensorValues sensor_values;
void callback(const beginner_tutorials::LightSensorValues::ConstPtr& msg)
{
sensor_values = *msg;
}
class Wall_Trace
{
Wall_Trace():
{
ros::Publisher cmd_vel = nh.advertise<geometry_msgs::Twist>("/cmd_vel" , 1);
ros::Subscriber sub = nh.subscribe("SensorValues",5,callback);
}
# ロボットを動かす関数
void run()
{
ros::Rate rate(10.0);
geometry_msgs::Twist data;
int a=0; //フラグ
while (ros::ok())
{
ros::spinOnce();
data.linear.x = 0.2;
data.angular.z = 0;
# 壁に当たるまで直進
if(sensor_values.sum_all < 300){
data.linear.x = 0.2;
data.angular.z = 0;
}
# 壁に当たると時計回り90度
if(sensor_values.sum_all >= 300 && a==0){
data.linear.x = 0.2;
data.angular.z = -M_PI*4/5;
sleep(0.1);
a=1;
}
# 右に旋回
if(sensor_values.left_side > 200 && a==1){
data.linear.x = 0;
data.angular.z = -M_PI*2/3;
}
# 左に旋回
if(sensor_values.left_side < 50 && a==1){
data.linear.x = 0;
data.angular.z = M_PI*2/3;
}
# 角に来た時に少し直進してから曲がる
if(sensor_values.left_side < 50 && sensor_values.left_forward <80 && a== 1){
data.linear.x = 0.2;
sleep(0.3);
data.angular.z = M_PI/2.3;
}
# 上で得たdataをcmd_velにパブリッシュ
cmd_vel.publish(data);
rate.sleep();
}
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv,"wall_trace");
ros::NodeHandle nh;
ros::ServiceClient client_1 = nh.serviceClient<std_srvs::Trigger>("motor_on");
ros::ServiceClient client_2 = nh.serviceClient<std_srvs::Trigger>("motor_off");
bool ros::service::waitForService(const std::string& motor_on, int 32_t timeout)
{
return waitForService(motor_on, ros::Duration(timeout / 1000.0));
}
bool ros::service::waitForService(const std::string& motor_off, int 32_t timeout)
{
return waitForService(motor_off, ros::Duration(timeout / 1000.0));
}
std_srvs::Trigger srv;
client_1.call(srv);
Wall_Trace robot_test;
# ロボットの移動開始
robot_test.run();
}
msgとsrv
LightSensorValues.msg
int16 right_forward
int16 right_side
int16 left_side
int16 left_forward
int16 sum_all
int16 front_average
Motor_freqs.msg
int16 left
int16 right
TimeMotion.srv
int16 left_hz
int16 right_hz
uint32 duration_ms
---
bool success
###ここに問題・エラーに対して試したことを記載してください。
pythonの方では動くことが確認できています．
C++のコードの方は，webからまねして書いたものもあるので全て理解はできていないです．
問題点として，一通りプログラムを書いた後に，catkin buildした時に進行度が91%のまま進まなくなっていて，このコードがあってるのか間違っているのかが分からない状態です．
もし，詳しい方がいましたら教えてくださるとうれしいです．
0
