Skip to content

Update NiryoConnection.html #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
henry9804 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Update NiryoConnection.html #2

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
263 changes: 242 additions & 21 deletions NiryoConnection.html
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,24 @@
<meta charset="utf-8" />
<script src="http://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
<script>
//url 설정
var niryo_url = 'ws://192.168.0.15:9090'

/*url 변경을 위한 코드 아직 구현되지 않음
//android에 현재 url값 반환
function get_url(){
return niryo_url;
}

//android에서 url입력 받아 업데이트
function set_url(url){
niryo_url = 'ws://' + url;
}
*/

// ROS 정의
var ros = new ROSLIB.Ros({
url: 'ws://192.168.0.33:9090'
url: niryo_url
});

var connected = 0;
Expand Down Expand Up @@ -550,6 +565,183 @@
});
}

//android에서 hsv값 담아 호출
function grabCapsule_w_param(capsule_data) {
if (connected == 0) {
document.getElementById("color").textContent = "Not connected"
return "fail: Niryo not connected";
}
//android에서 전달받은 인자 tokenize
hsv = capsule_data.split(' ');
h = parseInt(hsv[0]);
s = parseInt(hsv[1]);
v = parseInt(hsv[2]);
hdif = parseInt(hsv[3]);
sdif = parseInt(hsv[4]);
vdif = parseInt(hsv[5]);
console.log(h + ', ' + s + ', ' + v + ', ' + hdif + ', ' + sdif + ', ' + vdif);

toolnum = 0;
current_tool_id.subscribe(function(message) {
toolnum = message.data;
console.log('tool id: ' + message.data);
current_tool_id.unsubscribe();
});
// 기본적으로 Gripper3을 사용하고 있다고 가정함
var toolrequest = new ROSLIB.ServiceRequest({
value: 13
});
select_tool_service.callService(toolrequest, function(result) {
console.log(result.message);
});
var MoveActionClient = new ROSLIB.ActionClient({
ros: ros,
serverName: '/niryo_one/commander/robot_action',
actionName: 'niryo_one_msgs/RobotMoveAction'
});

// color_detection 서비스를 이용해 원하는 색 범위 내에 있는 이미지 추출, x, y, yaw 반환
// x, y, yaw 값을 get_target_pose에 전달해 workspace 내에서 상대적인 위치 반환
// 이 위치로 이동
// grippper 조절, move joint 등 이용
var color_detection = new ROSLIB.Service({
ros: ros,
name: '/niryo_one_vision/color_detection',
serviceType: 'niryo_one_msgs/CapsuleColor'
});

console.log(h + ', ' + s + ', ' + v);
x = 0;
y = 0;
z = 0;
roll = 0;
pitch = 0;
yaw = 0;
var HSV = new ROSLIB.ServiceRequest({
h: h,
s: s,
v: v,
hdif: hdif,
sdif: sdif,
vdif: vdif
});

var get_target_pose = new ROSLIB.Service({
ros: ros,
name: '/niryo_one/pose_converter/get_target_pose',
serviceType: 'niryo_one_msgs/GetTargetPose'
});
color_detection.callService(HSV, function(result) {
img = document.getElementById("contour");
img.src = "data:image/jpg;base64," + result.img.data;
x = result.obj_pose.x;
y = result.obj_pose.y;
yaw = result.obj_pose.yaw;
console.log(x + ', ' + y + ', ' + yaw);
var target_request = new ROSLIB.ServiceRequest({
workspace: "workspace",
grip: "auto",
tool_id: 13,
height_offset: 0.05,
x_rel: x,
y_rel: y,
yaw_rel: yaw,
});
get_target_pose.callService(target_request, function(result) {
var canvas = document.getElementById("canvas");
var ctx = canvas.getContext("2d");
ctx.drawImage(img, 0, 0);
x = result.target_pose.position.x + 0.02;
y = result.target_pose.position.y;
z = result.target_pose.position.z;
roll = result.target_pose.rpy.roll;
pitch = result.target_pose.rpy.pitch;
yaw = result.target_pose.rpy.yaw;
console.log(x + ', ' + y + ', ' + z + ', ' + roll + ', ' + pitch + ', ' + yaw);
if (Math.round(x * 1000) == 387 && Math.round(y * 1000) == 75) {
console.log('Capsule not found or too dark.');
return "fail: Capsule not found or too dark.";
}
var goal1 = new ROSLIB.Goal({
actionClient: MoveActionClient,
goalMessage: {
cmd: {
cmd_type: 2,
position: {
x: x,
y: y,
z: z
},
rpy: {
roll: roll,
pitch: pitch,
yaw: yaw
}
}
}
});
var goal2 = new ROSLIB.Goal({
actionClient: MoveActionClient,
goalMessage: {
cmd: {
cmd_type: 2,
position: {
x: x,
y: y,
z: z - 0.03
},
rpy: {
roll: roll,
pitch: pitch,
yaw: yaw
}
}
}
});
var open = new ROSLIB.Goal({
actionClient: MoveActionClient,
goalMessage: {
cmd: {
cmd_type: 6,
tool_cmd: {
tool_id: 13,
cmd_type: 1,
gripper_open_speed: 100
}
}
}
});
var close = new ROSLIB.Goal({
actionClient: MoveActionClient,
goalMessage: {
cmd: {
cmd_type: 6,
tool_cmd: {
tool_id: 13,
cmd_type: 2,
gripper_close_speed: 100
}
}
}
});
goal1.on('result', function(result) {
// console.log('Final result: ' + result.status);
open.send();
open.on('result', function() {
goal2.send();
goal2.on('result', function() {
close.send();
close.on('result', function() {
MoveZero();
});
});
});
});
goal1.send();
});
});
return "success";
}

// 색 범위 추출용 전역 변수
var firstx;
Expand All @@ -562,24 +754,48 @@
img = document.getElementById("image");
ctx.clearRect(0, 0, 640, 480);
ctx.drawImage(img, 0, 0);
var x = event.layerX;
var y = event.layerY;
//console.log(x + ', ' + y);

//touchevent 추가
if(event.type == "touchstart"){
var rect = event.target.getBoundingClientRect();
var x = Math.round(event.targetTouches[0].clientX - rect.left);
var y = Math.round(event.targetTouches[0].clientY - rect.top);
}
else{
var x = event.offsetX;
var y = event.offsetY;
}
console.log(x + ', ' + y);

firstx = x;
firsty = y;
}
// canvas mouseup 시 x, y 좌표와 firstx, firsty를 바탕으로 색 범위 계산, zoom 캔버스에 출력
function secondpoint(event) {
var canvas = document.getElementById("canvas");
var ctx = canvas.getContext("2d");

//touchevent 추가
if(event.type == "touchend"){
var rect = event.target.getBoundingClientRect();
var x = Math.round(event.changedTouches[0].clientX - rect.left);
var y = Math.round(event.changedTouches[0].clientY - rect.top);
}
else{
var x = event.offsetX;
var y = event.offsetY;
}
console.log(x + ', ' + y);

// 캔버스에 아무 이미지도 없으면 아무 동작도 하지 않음
if (!ctx.getImageData(0, 0, canvas.width, canvas.height).data.some(channel => channel !== 0)) {
document.getElementById("color").textContent = "Take Picture First"
ctx.strokeRect(firstx, firsty, x - firstx, y - firsty);
//android의 capsule 색 저장 함수 호출
window.android.addCapsule("255 255 255 0 0 0");
return;
}
var x = event.layerX;
var y = event.layerY;
// console.log(x + ', ' + y);

secondx = x;
secondy = y;
positiongrab = 0;
Expand Down Expand Up @@ -721,6 +937,9 @@
}

ctx.strokeRect(smallx, smally, largex - smallx, largey - smally);

//android의 capsule 색 저장 함수 호출
window.android.addCapsule(midH + ' ' + midS + ' ' + midV + ' ' + Hdif + ' ' + Sdif + ' ' + Vdif);
}

// RGB에서 HSV로 변환하는 함수
Expand Down Expand Up @@ -1091,7 +1310,7 @@
});
});
}
// (Switch on), Turn on the coffee machine(Press Lungo Button), wait for 30 sec, Wash machine: (Press Lungo Button & Wait for 30 sec)*3 times
// (Switch on), Turn on the coffee machine(Press Lungo Button), wait for 30 sec, Wash machine: (Press Lungo Button & Wait for 30 sec)*3 times
function TurnOnCoffeeMachine() {

}
Expand Down Expand Up @@ -1336,7 +1555,7 @@
});
}


// 기능 Test
// 아래 PressSwitch(), PressButtons(), HoldCup(), PutCupOnTemi() 는 임시로 단계별 기능 확인용으로 만든 함수
// 추후에 버튼으로 클릭할 것은 통합된 함수들 TurnOnCoffeeMachine(), MakeLungo(), MakeEspresso()
Expand Down Expand Up @@ -1368,14 +1587,14 @@
}
}
});

pressSwitch_pre1.send();
pressSwitch_pre1.on('result', function() {
pressSwitch_0.send();
pressSwitch_0.on('result', function() {
pressSwitch_pre1_cp.send();
pressSwitch_pre1_cp.on('result', function() {
MoveZero();
MoveZero();
});
});
});
Expand Down Expand Up @@ -1440,7 +1659,7 @@
}
}
});

pressEspressoLungo_pre2.send();
pressEspressoLungo_pre2.on('result', function() {
pressLungo_pre1.send();
Expand All @@ -1460,6 +1679,8 @@
});
});
});
//임시, 추후 MakeLungo로 android에서 호출 및 return 하는 함수 수정해야함
return "finish";
}
function HoldCup() {
var holdCup_pre2 = new ROSLIB.Goal({
Expand Down Expand Up @@ -1489,14 +1710,14 @@
}
}
});

holdCup_pre2.send();
holdCup_pre2.on('result', function() {
holdCup_pre1.send();
holdCup_pre1.on('result', function() {
holdCup_0.send();
holdCup_0.on('result', function() {
//MoveZero();
//MoveZero();
});
});
});
Expand Down Expand Up @@ -1530,14 +1751,14 @@
}
}
});

putCupOnTemi_pre1.send();
putCupOnTemi_pre1.on('result', function() {
putCupOnTemi_0.send();
putCupOnTemi_0.on('result', function() {
putCupOnTemi_post1.send();
putCupOnTemi_post1.on('result', function() {
//MoveZero();
//MoveZero();
});
});
});
Expand Down Expand Up @@ -1645,7 +1866,7 @@ <h2>ROS Webserver connection to Niryo</h2>
</fieldset>
</fieldset>
<fieldset style="border: 1px solid #FFFFFF">
<canvas id="canvas" style="float:left; border: 1px solid black;" crossorigin="anonymous" width="640" height="480" onmousedown="firstpoint(event)" onmouseup="secondpoint(event)"></canvas>
<canvas id="canvas" style="float:left; border: 1px solid black;" crossorigin="anonymous" width="640" height="480" onmousedown="firstpoint(event)" onmouseup="secondpoint(event)" ontouchstart="firstpoint(event)" ontouchend="secondpoint(event)"></canvas>
<FORM id="wrapper" style="width:200px; float:left; text-align:center; border: 1 solid black">
<div id="color">Drag to set HSV range</div>
<div id="h" style='display:inline;'></div>
Expand All @@ -1663,21 +1884,21 @@ <h2>ROS Webserver connection to Niryo</h2>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Put Down Capsule' , onclick='PutDownCapsule()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Put In Capsule' , onclick='PutInCapsule()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Rest' , onclick='Rest()'>

<!-- 임시 구현 함수 확인용 버튼, 실제 사용 시에는 버튼으로 나오지 않음 -->
<INPUT type='BUTTON' style="WIDTH:100pt" value='Press Switch' , onclick='PressSwitch()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Press Buttons' , onclick='PressButtons()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Hold Cup' , onclick='HoldCup()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Put Cup on Temi' , onclick='PutCupOnTemi()'>

<!-- 실제 사용할 버튼 -->
<!-- <INPUT type='BUTTON' style="WIDTH:100pt" value='Turn On CoffeeMachine' , onclick='TurnOnCoffeeMachine()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Make Coffee - Lungo' , onclick='MakeLungo()'>
<INPUT type='BUTTON' style="WIDTH:100pt" value='Make Coffee - Espresso' , onclick='MakeEspresso()'> -->

</FORM>
</fieldset>
<img id="contour" style="display:none">
</body>

</html>
</html>