ROS
V-REP

ROSとV-REPの連携 その3.tf問題編

More than 1 year has passed since last update.

前回の記事ではjoint_stateをpublishしましたが今回はtfをpublishしてみました。
mainスクリプトを以下のように変更します。

-- 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) ---------
function get_transform_stamped(object_handle,child_frame_id,rel_object_handle,parent_frame_id)
    t=simGetSystemTime()
    p=simGetObjectPosition(object_handle,rel_object_handle)
    o=simGetObjectQuaternion(object_handle,rel_object_handle)

    return {
        header={
            stamp=t,
            frame_id=parent_frame_id
        },
        child_frame_id=child_frame_id,
        transform={
            -- ROS has definition x=front y=side z=up
            translation={x=p[1],y=p[2],z=p[3]},--V-rep
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}--v-rep
        }
    }
end

if (sim_call_type==sim_mainscriptcall_initialization) then
    simHandleSimulationStart()
    simOpenModule(sim_handle_all)
    simHandleGraph(sim_handle_all_except_explicit,0)
    --create ros publisher
    joint_pub = simExtRosInterface_advertise('/joint_state','sensor_msgs/JointState')
    --create object handle
    joint_handle = simGetObjectHandle('Revolute_joint')
    Cuboid0_handle = simGetObjectHandle('Cuboid0')
    Cuboid1_handle = simGetObjectHandle('Cuboid1')
    DefaultFloor_Handle = simGetObjectHandle('DefaultFloor')
    Vision_Sensor_Handle = simGetObjectHandle('Vision_sensor')
    --debug window
    myconsole=simAuxiliaryConsoleOpen('Text_Debugger',10,0,{0.5,0.9},{0.2,0.2},NULL,NULL)
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())

    --publish joint state
    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)

    --publish tf
    simExtRosInterface_sendTransform(get_transform_stamped(Cuboid1_handle,'Cuboid1',DefaultFloor_Handle,'base_footprint'))
    simExtRosInterface_sendTransform(get_transform_stamped(Cuboid0_handle,'Cuboid0',Cuboid1_handle,'Cuboid1'))
    simExtRosInterface_sendTransform(get_transform_stamped(joint_handle,'Revolute_joint',Cuboid1_handle,'Cuboid1'))
    simExtRosInterface_sendTransform(get_transform_stamped(Vision_Sensor_Handle,'Vision_sensor',Cuboid1_handle,'Cuboid1'))
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
function get_transform_stamped(object_handle,child_frame_id,rel_object_handle,parent_frame_id)

この関数でgeometry_msgs/TransformStampedを計算しています。
simExtRosInterface_sendTransform関数で計算したtransformをbroadcastします。
それではこのスクリプトにmainスクリプトを差し替えたらシミュレーションを回してみましょう。
Screenshot from 2017-08-10 21:09:49.png
rvizで確認すると下のようにtfがちゃんと出ています。(下の画像をクリックすると動画が再生されます.)
Failed to opne the video

今回とりあえずなんとかtfを出力するところまでは出来ました。
今後は他のセンサーをいろいろと使えるようになりたいですね。