ROSとV-REPの連携 その2.joint_state問題編

今回のお題

前回の記事ではROSとV-REPを連携させてノードを立ち上げるところまで行きました。
しかしいろいろ不足点(tfとかjoint_stateが出てない・・・)等がありましたのでそのあたりを解決したいと思います。
今回はとりあえずjoint_stateをpublishさせてみます。
今回は前回使用したbaxterではなく「rosInterfaceTopicPublisherAndSubscriber.ttt」というシーンを使用したいと思います。

こんな感じの謎の棒ロボットがくるくる回るシーンです。

Joint_Stateの追加

とりあえずJoint_Stateの追加から試してみます。
こちらの公式フォーラムを参考にしました。
とりあえずLuaのスクリプトにROSのAPIを追加すれば良い模様・・・
書類みたいなアイコンをクリックするとスクリプトが編集できるようになりました。

追記したのは

pub = simExtRosInterface_advertise('/joint_state','sensor_msgs/JointState')

この一行です。
これ一行を追記することでjoint_stateトピックへ情報を流すPublisherが作れました。
当然ですがこのままではデータはpublishされません。
そこで以下のようにスクリプトを変更しました。

NonThreadedChildScript(Revolute_joint)

if (sim_call_type==sim_childscriptcall_initialization) then
    h=simGetObjectAssociatedWithScript(sim_handle_self)
    joint_pub = simExtRosInterface_advertise('/joint_state','sensor_msgs/JointState')
    joint_handle=simGetObjectHandle('Revolute_joint')
end


if (sim_call_type==sim_childscriptcall_actuation) then
    local t=simGetSimulationTime()
    simSetJointPosition(h,90*math.pi*math.sin(0.25*t)/180)
    local effort = simGetJointForce(joint_handle)
    local position = simGetJointPosition(joint_handle)
    local result,velocity=simGetObjectFloatParameter(joint_handle,2012)
    joint_state = {}
    joint_state['header']={seq=0,stamp=simExtRosInterface_getTime(), frame_id='Revolute_joint'}
    joint_state['name'] = {'Revolute_joint'}
    joint_state['position'] = {position}
    joint_state['velocity'] = {velocity}
    joint_state['effort'] = {effort}
    simExtRosInterface_publish(joint_pub,joint_state)
end

これで実行してみると

joint_stateがpublishされました。

とりあえずうまく行ったのでこれをメインスクリプトに書き込みました。
まずNonThreadedChildScript(Revolute_joint)を最初の状態に戻し、Main Scriptを開きます。

MainScript

-- This is the main script. The main script is not supposed to be modified,
-- unless there is a very good reason to do it.
-- The main script is called at each simulation pass. Without main script,
-- there is no real simulation (child scripts are not called either in that case).
-- A main script marked as "default" (this is the default case) will use the
-- content of following file: system/dltmscpt.txt. This allows your old simulation
-- scenes to be automatically also using newer features, without explicitely coding
-- them. If you modify the main script, it will be marked as "customized", and you
-- won't benefit of that automatic forward compatibility mechanism. 

-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)

-- Initialization part (executed just once, at simulation start) ---------
if (sim_call_type==sim_mainscriptcall_initialization) then
    simHandleSimulationStart()
    simOpenModule(sim_handle_all)
    simHandleGraph(sim_handle_all_except_explicit,0)
    joint_pub = simExtRosInterface_advertise('/joint_state','sensor_msgs/JointState')
    joint_handle=simGetObjectHandle('Revolute_joint')
end
--------------------------------------------------------------------------

-- Regular part (executed at each simulation step) -----------------------
if (sim_call_type==sim_mainscriptcall_regular) then
    -- "Actuation"-part --
    simResumeThreads(sim_scriptthreadresume_default)
    simResumeThreads(sim_scriptthreadresume_actuation_first)
    simLaunchThreadedChildScripts()
    simHandleChildScripts(sim_childscriptcall_actuation)
    simResumeThreads(sim_scriptthreadresume_actuation_last)
    simHandleCustomizationScripts(sim_customizationscriptcall_simulationactuation)
    simHandleModule(sim_handle_all,false)
    simHandleJoint(sim_handle_all_except_explicit,simGetSimulationTimeStep()) -- DEPRECATED
    simHandlePath(sim_handle_all_except_explicit,simGetSimulationTimeStep()) -- DEPRECATED
    simHandleMechanism(sim_handle_all_except_explicit)
    simHandleIkGroup(sim_handle_all_except_explicit)
    simHandleDynamics(simGetSimulationTimeStep())
    simHandleMill(sim_handle_all_except_explicit)

    -- "Sensing"-part --
    simHandleSensingStart()
    simHandleCollision(sim_handle_all_except_explicit)
    simHandleDistance(sim_handle_all_except_explicit)
    simHandleProximitySensor(sim_handle_all_except_explicit)
    simHandleVisionSensor(sim_handle_all_except_explicit)
    simResumeThreads(sim_scriptthreadresume_sensing_first)
    simHandleChildScripts(sim_childscriptcall_sensing)
    simResumeThreads(sim_scriptthreadresume_sensing_last)
    simHandleCustomizationScripts(sim_customizationscriptcall_simulationsensing)
    simHandleModule(sim_handle_all,true)
    simResumeThreads(sim_scriptthreadresume_allnotyetresumed)
    simHandleGraph(sim_handle_all_except_explicit,simGetSimulationTime()+simGetSimulationTimeStep())
    local effort = simGetJointForce(joint_handle)
    local position = simGetJointPosition(joint_handle)
    local result,velocity=simGetObjectFloatParameter(joint_handle,2012)
    joint_state = {}
    joint_state['header']={seq=0,stamp=simExtRosInterface_getTime(), frame_id='Revolute_joint'}
    joint_state['name'] = {'Revolute_joint'}
    joint_state['position'] = {position}
    joint_state['velocity'] = {velocity}
    joint_state['effort'] = {effort}
    simExtRosInterface_publish(joint_pub,joint_state)
end
--------------------------------------------------------------------------

-- Clean-up part (executed just once, before simulation ends) ------------
if (sim_call_type==sim_mainscriptcall_cleanup) then
    simResetMilling(sim_handle_all)
    simResetMill(sim_handle_all_except_explicit)
    simResetCollision(sim_handle_all_except_explicit)
    simResetDistance(sim_handle_all_except_explicit)
    simResetProximitySensor(sim_handle_all_except_explicit)
    simResetVisionSensor(sim_handle_all_except_explicit)
    simCloseModule(sim_handle_all)
end
--------------------------------------------------------------------------

-- By default threaded child scripts switch back to the main thread after 2 ms. The main
-- thread switches back to a threaded child script at one of above's "simResumeThreads"
-- location

これで毎ステップごとにJointStateがPublishされるようになりました。

次回はtfをpublishします。