[Ros Web] Roslib , Ros2djs를 활용한 web interface 만들기 #2 코드설명

2022.05.12 - [Ros] - [Ros Web] Roslib , Ros2djs를 활용한 web interface 만들기 #1 전체코드

[Ros Web] Roslib , Ros2djs를 활용한 web interface 만들기 #1 전체코드

* 앞에서 사용한 코드에 대한 설명으로 주된 설명은 roslib / ros2djs의 api를 사용하는 방법에 대한 설명이다.

1) ros websocket server 연결

-> websocket server가 port 9090에서 생성시킨 다음 websocket server와 연결하기 위하여 ROSLIB.ROS 함수를 사용한다.

연결이 된 것을 확인 하기 위하여 HTML을 사용하여 "Connected" 표시를 하여 사용자에게 알려준다.

var ros = new ROSLIB.Ros({
    url : 'ws://localhost:9090'
  });

  ros.on('connection', function() {
    document.getElementById("status").innerHTML = "Connected";
  });

  ros.on('error', function(error) {
    document.getElementById("status").innerHTML = "Error";
  });

  ros.on('close', function() {
    document.getElementById("status").innerHTML = "Closed";
  });

2) TOPIC 생성

-> Roslibjs 에서는 기존의 Ros와 동일하게 Topic안에 message를 담아서 전달한다. 그러므로 Topic을 publish 또는 subscribe 시에 Topic을 선언해 주어야 한다. 선언 방법은 연결되어 있는 websocket server에 등록하는 방식으로 선언한다.

//Pose Topic
const SlamPoseTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/tf',
  messageType:'tf2_msgs/TFMessage'
})

const NavPoseTopic = new ROSLIB.Topic({
    ros: ros,
    name: '/amcl_pose',
    messageType: 'geometry_msgs/PoseWithCovarianceStamped'
});

//Path Topic
var listenerforPath = new ROSLIB.Topic ({
  ros : ros,
  name : '/move_base/NavfnROS/plan',
  messageType : 'nav_msgs/Path'
});

//Trace Topic
var moveBaseFB = new ROSLIB.Topic ({
  ros : ros,
  name : '/move_base/feedback',
  messageType : 'move_base_msgs/MoveBaseActionFeedback'
});

3) Message 생성

 

-> message 생성은 기존 ros생성과 동일한 형태이며, message type과 동일한 객체로 생성해야한다. 아래의 메시지의 type은 

geometry_msgs/PoseWithCovarianceStamped 형태로 해당 type에 들어있는 형태와 동일한 객체로 아래와 같이 생성한다.

var posestamped_msg = new ROSLIB.Message({
    header: {
      stamp: {
        secs : 0, 
        nsecs : 100
      },
      frame_id : "map"              
    },
    pose: {
     pose:{
      position: {
        x : pose_x,
        y : pose_y,
        z : 0.0
      },
      orientation: orientation
     }
      ,
      covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853892326654787]
    },
  });

4) Viewer 생성

-> roslibjs의 동작방법은 Viewer 라는 객체를 만들고, 그안에 scene라는 rootObject를 만들어서 그 안에 생성되는 UI들을 넣어서 화면을 보여주는 형태를 띈다. 즉 Viwer 안에 있는 scene이 계속 변경되면서 그림을 그려주는 형태로 로봇의 위치나 경로, trace 형태를 표시해준다.

 //map server로 부터 map 정보를 가져와서 보여준다. 
  var viewer = new ROS2D.Viewer({
    divID : 'map',
    width : 700,
    height : 700,
  });
  
 // 실제 map에 들어가는 정보를 표시하기위한 client를 생성한다.
 // 뒤에서 설명하겠지만 여기에다가 이벤트 리스터를 등록하여 변동을 감지하여 계속 UI를 그려주는 식으로
 // 동작하게 된다.
 // Setup the map client.
  var gridClient = new ROS2D.OccupancyGridClient({
    ros : ros,
    rootObject : viewer.scene,
    continuous: true
  });
  
// 로봇의 위치를 Marker로 표시
// 여기서는 위치뿐만 아니라 로봇의 방향을 표기하기 위하여 ArrowShape을 사용하였다.
// robot odometry
var robotMarker = new ROS2D.ArrowShape({
  size : 0.7,
  strokeSize : 0.01,
  pulse: true,
  fillColor: createjs.Graphics.getRGB(255,0,0, 0.9),
});

// 로봇의 경로를 표시하기 위하여 PathShape를 정의한다.
// PathShape
var pathShape = new ROS2D.PathShape({
  strokeSize : 0.03,
  strokeColor : createjs.Graphics.getRGB(0, 255, 0,1),
  });

5) Viewer 에 등록

// 생성한 Viewer 인자들을 앞에서 정의한 viewer.scene안에 넣어준다.
gridClient.rootObject.addChild(pathShape);
gridClient.rootObject.addChild(traceShape);
gridClient.rootObject.addChild(robotMarker);

6) 이벤트리스너 등록

// Robot의 pose를 설정하기 위해서 mousedown -> mousemove --> mouseup에 각각의 
// 이벤트를 등록해서 브라우저상의 map안에서 어떤 좌표를 클릭했는 지 파악한 뒤 함수를 통해
// Topic으로 위치정보 msg를 담아서 보낸다.
// 3가지 경우로 이벤트를 나누는 이유는 마우스의 down-> move -> up 에 해당 하는 각각의 위치정보를
// 받아서 위치 뿐만아니라 방향에 대한 정보를 받기 위해서 3가지 형태로 분류하였다.

viewer.scene.addEventListener('stagemousedown', function(event) {
  let initialPoseChecked = document.querySelector("#initialPoseswitch").checked
  let goalPoseChecked = document.querySelector("#goalPoseswitch").checked
  // set Btn control
  let operMode=initialPoseChecked?"initial":"goal"
  // button to set inital pose
  if(initialPoseChecked){
    document.querySelector("#goalPoseswitch").checked=false
    mouseEventHandler(event,'down',operMode);
  }

  if(goalPoseChecked){
    document.querySelector("#initialPoseswitch").checked=false
    mouseEventHandler(event,'down',operMode);
  }

});

viewer.scene.addEventListener('stagemousemove', function(event) {
  let initialPoseChecked = document.querySelector("#initialPoseswitch").checked
  let goalPoseChecked = document.querySelector("#goalPoseswitch").checked
  let operMode=initialPoseChecked?"initial":"goal"
  // button to set inital pose
  if(initialPoseChecked){
    document.querySelector("#goalPoseswitch").checked=false
    mouseEventHandler(event,'move',operMode);
  }

  if(goalPoseChecked){
    document.querySelector("#initialPoseswitch").checked=false
    mouseEventHandler(event,'move',operMode);
  }
  
});

viewer.scene.addEventListener('stagemouseup', function(event) {
  let initialPoseChecked = document.querySelector("#initialPoseswitch").checked
  let goalPoseChecked = document.querySelector("#goalPoseswitch").checked
  let operMode=initialPoseChecked?"initial":"goal"
  // button to set inital pose
  if(initialPoseChecked){
    document.querySelector("#goalPoseswitch").checked=false
    mouseEventHandler(event,'up',operMode);
  }

  if(goalPoseChecked){
    document.querySelector("#initialPoseswitch").checked=false
    mouseEventHandler(event,'up',operMode);
  }
  
});


// Path에 대한 listener 등록
  listenerforPath.subscribe((message)=> {
    if(message){
      pathShape.setPath(message);
    }
    // listenerforPath.unsubscribe();
  });
  
  // Trace에 대한 listener 등록
    moveBaseFB.subscribe(function(message) {
      traceShape.addPose(message.feedback.base_position.pose);
    });
    
  // 전체 scene에 대한 listener 등록
  // 여기서 전체 scene에 대한 listener를 등록하여 이벤트 발생 시 로봇의 계속된 위치를 표시해준다.
  // Scale the canvas to fit to the map
  gridClient.on('change', function(){
    viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
    viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
  });
}

7) 각종 함수

-> 위에 Topic과 메시지에 대한 정보를 eventListener로 받아온다음에 실제로 화면에 어떤 정보를 보여줄지를 결정하는 함수이다. mouse event를 감지하기 위한 함수 및 로봇의 위치를 설정하기 위한 함수들이 있으며, 특히 api를 통해 받아오는 JSON 값의 형태에 따라서 함수를 잘 변경해서 사용해야한다.

// 초기위치 설정
// create initial Pose Topic and msg
const creatInitialPose=(pose_x,pose_y,orientation)=>{
  const initialPose = new ROSLIB.Topic({
    ros: ros,
    name: '/initialpose',
    messageType: 'geometry_msgs/PoseWithCovarianceStamped'
  });
  
  var posestamped_msg = new ROSLIB.Message({
    header: {
      stamp: {
        secs : 0, 
        nsecs : 100
      },
      frame_id : "map"              
    },
    pose: {
     pose:{
      position: {
        x : pose_x,
        y : pose_y,
        z : 0.0
      },
      orientation: orientation
     }
      ,
      covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853892326654787]
    },
  });
   initialPose.publish(posestamped_msg)
    console.log("initialPose publish")
}


// 목표위치 설정
// create Goal Pose Topic and msg
const creatGoalPose=(pose_x,pose_y,orientation)=>{
  const goalPose = new ROSLIB.Topic({
    ros: ros,
    name: '/move_base_simple/goal',
    messageType: 'geometry_msgs/PoseStamped'
  });
  
  var posestamped_msg = new ROSLIB.Message({
    header: {
      stamp: {
        secs : 0, 
        nsecs : 100
      },
      frame_id : "map"              
    },
    pose: {
      position: {
        x : pose_x,
        y : pose_y,
        z : 0.0
      },
      orientation: orientation
     }
  });
    goalPose.publish(posestamped_msg)
    console.log("goalPose publish")
}


// slam 혹은 navigation에 따라 받아오는 pose 토픽이 다르므로 각각을 선택할 수있게 하였다.
// slam은 cartographer을 사용하였고, navigation은 /amcl의 pose를 받아오게 하였다.
  let OperRatingMode="nav"

  const CreatePoseTopic=(OperRatingMode)=>{

    console.log(`Create posetopic , mode :${OperRatingMode}`)

    if(OperRatingMode == "slam"){
      console.log("poseTopic slam")
        // Make robot pose subscriber
    const SlamPoseTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/tf',
      messageType:'tf2_msgs/TFMessage'
      // messageType : 'turtlesim/Pose'
      // messageType : 'geometry_msgs/Pose'
    })
    return SlamPoseTopic

    }else if(OperRatingMode=="nav"){
      console.log("poseTopic nav")
      const NavPoseTopic = new ROSLIB.Topic({
        ros: ros,
        name: '/amcl_pose',
        messageType: 'geometry_msgs/PoseWithCovarianceStamped'
      });
      return NavPoseTopic
    }
  }
  
  // 로봇의 위치를 설정하기 위해서 mouse event를 감지하는 함수
  var mouseEventHandler = function(event, mouseState,operMode) {
   console.log(`mouseState :${mouseState}`)
  if (mouseState === 'down'){
    mouseDown=true
    console.log("mouse down")
    // get position when mouse button is pressed down
    mouseDownPosition = viewer.scene.globalToRos(event.stageX, event.stageY);
    mouseDownPositionVec3 = new ROSLIB.Vector3(mouseDownPosition);
    mouseDownPose = new ROSLIB.Pose({
      position: new ROSLIB.Vector3(mouseDownPositionVec3)
    });
    console.log(mouseDownPose.position)
  }
  else if (mouseState === 'move' && mouseDown){
    console.log("mouse move")
    // // remove obsolete orientation marker
    gridClient.rootObject.removeChild(robotMarker);
  }
  else if (mouseState === 'up'&& mouseDown){
    mouseDown=false
    mouseUpPosition = viewer.scene.globalToRos(event.stageX, event.stageY);
    mouseUpPositionVec3 = new ROSLIB.Vector3(mouseUpPosition);
    const mouseUpPose = new ROSLIB.Pose({
      position: new ROSLIB.Vector3(mouseUpPositionVec3)
    });

    // upPose - DownPose
    xDelta =  mouseUpPose.position.x - mouseDownPose.position.x ;
    yDelta =  mouseUpPose.position.y - mouseDownPose.position.y;

    thetaRadians  = Math.atan2(xDelta,yDelta);

    thetaDegrees = thetaRadians * (180.0 / Math.PI);
          
    if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
      thetaRadians += (3 * Math.PI / 2);
    } else {
      thetaRadians -= (Math.PI/2);
    }

    var qz =  Math.sin(-thetaRadians/2.0);
    var qw =  Math.cos(-thetaRadians/2.0);
    // degree convert to quaternion
    var orientation = new ROSLIB.Quaternion({x:0, y:0, z:qz, w:qw});

    // console.log(`xDelta : ${xDelta}, yDelta : ${yDelta} , degree : ${thetaDegrees}`)

    // set robotmaker
    if(operMode=="initial"){
      creatInitialPose(mouseDownPose.position.x,mouseDownPose.position.y,orientation)
    }else if (operMode=="goal")
      creatGoalPose(mouseDownPose.position.x,mouseDownPose.position.y,orientation)
  }};
  
  
 // 로봇의 위치를 실제 등록하는 함수
 // 여기서 중요한 것은 메시지에 따라서 받아오는 pose값이 다르므로 설정해 주어야 하며, 
 // ros에서 좌표계는 quternion을 사용하므로 theta 또는 radian으로 적절하게 변경해서 사용해야한다.
 const createFunc = function (handlerToCall, discriminator, robotMarker,OperRatingMode) {


  return discriminator.subscribe(function(pose){

      if (OperRatingMode=="slam"){
      // slam
      // CrtoGrapher slam case(tf2_msgs/TFMessage)
      console.log("slam work")
      let odomPose = pose.transforms[0].transform.translation
      let baseLinkPose=pose.transforms[1].transform.translation

      //  When using Nav,  gemometry_msgs/Pose .orientation. {x,y,z,w} (Quarternion)  
      //  When using SLAM  tf2_msgs/TFMessage .transform . rotation  {x,y,z,w} (quarternion)
      let quaZ=pose.transforms[1].transform.rotation.z

      // pose using odom
      robotMarker.x = baseLinkPose.x;
      robotMarker.y = -baseLinkPose.y;

      let degreeZ = 0;
      if( quaZ >= 0 ) {
          degreeZ = quaZ / 1 * 180
      } else {
          degreeZ = (-quaZ) / 1 * 180 + 180
      };
      // degree
      robotMarker.rotation = degreeZ;

      }else if(OperRatingMode=="nav"){
      // navigation
      console.log("nav work")
      robotMarker.x = pose.pose.pose.position.x;
      robotMarker.y = -pose.pose.pose.position.y;

      let orientationQuerter=pose.pose.pose.orientation
      var q0 = orientationQuerter.w;
      var q1 = orientationQuerter.x;
      var q2 = orientationQuerter.y;
      var q3 = orientationQuerter.z;
      degree=-Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) * 180.0 / Math.PI
      robotMarker.rotation = degree;
      }

        // rootObject를 통해서 robotMaker에 Marker 넣어줌
        gridClient.rootObject.addChild(robotMarker);

    })
}