Skip to content

Commit

Permalink
Merge pull request #24 from hcrlab/gazebo_sim
Browse files Browse the repository at this point in the history 
Gazebo sim and end-effector following
  • Loading branch information
@mayacakmak
mayacakmak authored Jun 25, 2021
2 parents 11028ea + cd1b569 commit a763551
Show file tree
Hide file tree
Showing 8 changed files with 226 additions and 52 deletions.
3 changes: 3 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,6 @@
*.so

node_modules

# VS Code related
.vscode/
1 change: 1 addition & 0 deletions launch/web_interface.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

<!-- ROSBRIDGE -->
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"></include>
<node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" respawn="false" output="screen"/>
<!-- -->

</launch>
2 changes: 1 addition & 1 deletion operator/operator.css
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
*/

.video_div div {
background-color: rgba(0,0,0,0.0);
background-color: rgba(0,0,0,0);
grid-row-start: 1;
grid-column-start: 1;
}
Expand Down
5 changes: 5 additions & 0 deletions operator/operator.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -376,6 +376,11 @@
<div id="interface-config">
<label> Automatically change view? </label>
<input id="autoViewOn" type="checkbox" onchange="toggleAutoView()">

<br>

<label> Camera automatically follow gripper </label>
<input id="cameraFollowGripperOn" type="checkbox" onchange="toggleCameraFollowGripper()">
</div>

</div>
Expand Down
2 changes: 1 addition & 1 deletion robot/robot.css
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
}

#video-display {
position:absolute;
/*position:absolute;*/
height:90%;
z-index:1;
}
Expand Down
25 changes: 25 additions & 0 deletions robot/robot.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,33 @@
<video id="video-display" autoplay></video>
</div>

<div>
<div id="simState"></div>
<button id="toggleSimState"></button>
</div>

<link rel="stylesheet" type="text/css" href="robot.css" media="screen" />

<script type='text/javascript'>
var inSim = localStorage.getItem('inSim') == "true" ? true : false;

if (inSim) {
document.getElementById("simState").innerHTML = "You are running in simulation mode";
document.getElementById("toggleSimState").textContent = "Switch to physical robot";
document.getElementById("toggleSimState").onclick = function () {
localStorage.setItem("inSim", false);
location.reload();
}
} else {
document.getElementById("simState").innerHTML = "You are running on the physical robot";
document.getElementById("toggleSimState").textContent = "Switch to simulation mode";
document.getElementById("toggleSimState").onclick = function () {
localStorage.setItem("inSim", true);
location.reload();
}
}
</script>

<script src="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script src="https://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
<script src="../shared/commands.js"></script>
Expand Down
219 changes: 169 additions & 50 deletions robot/ros_connect.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,13 @@ var messages_received_body = [];
var commands_sent_body = [];
var messages_received_wrist = [];
var commands_sent_wrist = [];
var rosImageReceived = false
var img = document.createElement("IMG")
img.style.visibility = 'hidden'
var rosJointStateReceived = false
var jointState = null
var rosImageReceived = false;
var img = document.createElement("IMG");
img.style.visibility = 'hidden';
var rosJointStateReceived = false;
var jointState = null;
var rosRobotStateReceived = false;
var robotState = null;

var session_body = {ws:null, ready:false, port_details:{}, port_name:"", version:"", commands:[], hostname:"", serial_ports:[]};

Expand All @@ -33,7 +35,7 @@ ros.on('close', function() {

var imageTopic = new ROSLIB.Topic({
ros : ros,
name : '/camera/color/image_raw/compressed',
name : inSim ? '/realsense/color/image_raw/compressed' : '/camera/color/image_raw/compressed', // ROS paths change depending on whether we're in a gazebo sim or running on stretch
messageType : 'sensor_msgs/CompressedImage'
});

Expand All @@ -57,19 +59,9 @@ imageTopic.subscribe(function(message) {
});


function getJointEffort(jointStateMessage, jointName) {
var jointIndex = jointStateMessage.name.indexOf(jointName)
return jointStateMessage.effort[jointIndex]
}

function getJointValue(jointStateMessage, jointName) {
var jointIndex = jointStateMessage.name.indexOf(jointName)
return jointStateMessage.position[jointIndex]
}

var jointStateTopic = new ROSLIB.Topic({
ros : ros,
name : '/stretch/joint_states/',
name : inSim ? '/joint_states/' : '/stretch/joint_states/',
messageType : 'sensor_msgs/JointState'
});

Expand All @@ -81,17 +73,6 @@ jointStateTopic.subscribe(function(message) {
console.log('Received first joint state from ROS topic ' + jointStateTopic.name);
rosJointStateReceived = true
}

// send wrist joint effort
var yawJointEffort = getJointEffort(jointState, 'joint_wrist_yaw')
var message = {'type': 'sensor', 'subtype':'wrist', 'name':'yaw_torque', 'value': yawJointEffort}
sendData(message)

// send gripper effort
var gripperJointEffort = getJointEffort(jointState, 'joint_gripper_finger_left')
var message = {'type': 'sensor', 'subtype':'gripper', 'name':'gripper_torque', 'value': gripperJointEffort}
sendData(message)

// Header header
// string[] name
// float64[] position
Expand All @@ -100,14 +81,54 @@ jointStateTopic.subscribe(function(message) {
//imageTopic.unsubscribe()
});

var tfClient = new ROSLIB.TFClient({
ros : ros,
fixedFrame : 'base_link',
angularThres : 0.01,
transThres : 0.01
});

var link_gripper_finger_left_tf;
tfClient.subscribe('link_gripper_finger_left', function(tf) {
link_gripper_finger_left_tf = tf;
});

var link_head_tilt_tf;
tfClient.subscribe('link_head_tilt', function(tf) {
link_head_tilt_tf = tf;
});

var trajectoryClient = new ROSLIB.ActionClient({
var trajectoryClients = {}
trajectoryClients.main = new ROSLIB.ActionClient({
ros : ros,
serverName : '/stretch_controller/follow_joint_trajectory',
serverName : inSim ? '/stretch_joint_state_controller/follow_joint_trajectory' : '/stretch_controller/follow_joint_trajectory',
actionName : 'control_msgs/FollowJointTrajectoryAction'
});

if (inSim) {
trajectoryClients.head = new ROSLIB.ActionClient({
ros : ros,
serverName : '/stretch_head_controller/follow_joint_trajectory',
actionName : 'control_msgs/FollowJointTrajectoryAction'
});

trajectoryClients.arm = new ROSLIB.ActionClient({
ros : ros,
serverName : '/stretch_arm_controller/follow_joint_trajectory',
actionName : 'control_msgs/FollowJointTrajectoryAction'
});

trajectoryClients.gripper = new ROSLIB.ActionClient({
ros : ros,
serverName : '/stretch_gripper_controller/follow_joint_trajectory',
actionName : 'control_msgs/FollowJointTrajectoryAction'
});

trajectoryClients.base = new ROSLIB.Topic({
ros: ros,
name: '/stretch_diff_drive_controller/cmd_vel',
messageType: 'geometry_msgs/Twist'
})
}

function generatePoseGoal(pose){

Expand All @@ -124,29 +145,88 @@ function generatePoseGoal(pose){
jointNames.push(key)
jointPositions.push(pose[key])
}
if (!inSim) {
var t = trajectoryClients.main;
} else {
switch (jointNames[0]) {
case 'joint_head_tilt':
case 'joint_head_pan':
var t = trajectoryClients.head;
break;
case 'joint_gripper_finger_left':
var t = trajectoryClients.gripper;
jointNames.push('joint_gripper_finger_right')
jointPositions.push(jointPositions[0])
break;
case 'wrist_extension':
console.log(jointNames, jointPositions)
jointNames = ['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']
jointPositions = [jointPositions[0]/4, jointPositions[0]/4, jointPositions[0]/4, jointPositions[0]/4]
case 'joint_lift':
case 'joint_wrist_yaw':
var t = trajectoryClients.arm;
break;
case 'translate_mobile_base':
var translate = new ROSLIB.Message({
linear : {
x : jointPositions[0]*10,
y : 0.0,
z : 0.0
},
angular : {
x : 0.0,
y : 0.0,
z : 0.0
}
});
trajectoryClients.base.publish(translate)
return {"send": function(){}};
case 'rotate_mobile_base':
var rotate = new ROSLIB.Message({
linear : {
x : 0.0,
y : 0.0,
z : 0.0
},
angular : {
x : 0.0,
y : 0.0,
z : jointPositions[0]*10
}
});
trajectoryClients.base.publish(rotate)
return {"send": function(){}};
}
}

var newGoal = new ROSLIB.Goal({
actionClient : trajectoryClient,
goalMessage : {
trajectory : {
joint_names : jointNames,
points : [
{
positions : jointPositions
}
]
}
}
})
actionClient : t,
goalMessage : {
trajectory : {
joint_names : jointNames,
points : [
{
positions : jointPositions,
time_from_start: {
secs: 0,
nsecs: 1
}
}
]
}
}
});


console.log('newGoal created =' + newGoal)

// newGoal.on('feedback', function(feedback) {
// console.log('Feedback: ' + feedback.sequence);
// });
newGoal.on('feedback', function(feedback) {
console.log('Feedback: ' + feedback.sequence);
});

// newGoal.on('result', function(result) {
// console.log('Final Result: ' + result.sequence);
// });
newGoal.on('result', function(result) {
console.log('Final Result: ' + result.sequence);
});

return newGoal
}
Expand Down Expand Up @@ -245,6 +325,18 @@ function baseTurn(ang_deg, vel) {
//sendCommandBody({type: "base",action:"turn", ang:ang_deg, vel:vel});
}

function getJointValue(jointStateMessage, jointName) {
if (inSim && jointName == 'wrist_extension') {
var value = 0;
for (var i = 0; i < 4; i++) {
var jName = ['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3'][i]
value += jointStateMessage.position[jointStateMessage.name.indexOf(jName)];
}
return value
}
var jointIndex = jointStateMessage.name.indexOf(jointName)
return jointStateMessage.position[jointIndex]
}

function sendIncrementalMove(jointName, jointValueInc) {
console.log('sendIncrementalMove start: jointName =' + jointName)
Expand All @@ -260,6 +352,33 @@ function sendIncrementalMove(jointName, jointValueInc) {
return false
}

function headLookAtGripper() {
console.log('attempting to send headLookAtGripper command')
if (link_gripper_finger_left_tf && link_head_tilt_tf) {

var posDifference = {
x: link_gripper_finger_left_tf.translation.x - link_head_tilt_tf.translation.x,
y: link_gripper_finger_left_tf.translation.y - link_head_tilt_tf.translation.y,
z: link_gripper_finger_left_tf.translation.z - link_head_tilt_tf.translation.z
};

// Normalize posDifference
var scalar = Math.sqrt(posDifference.x**2 + posDifference.y**2 + posDifference.z**2);
posDifference.x /= scalar;
posDifference.y /= scalar;
posDifference.z /= scalar;

var pan = Math.atan2(posDifference.y, posDifference.x);
var tilt = Math.atan2(posDifference.z, -posDifference.y);

var headFollowPoseGoal = generatePoseGoal({'joint_head_pan': pan, 'joint_head_tilt': tilt})
headFollowPoseGoal.send()
console.log('sending arm follow pose to head')
return true
}
return false
}

function armMove(dist, timeout, vel) {
console.log('attempting to sendarmMove command')
var jointValueInc = 0.0
Expand Down Expand Up @@ -393,4 +512,4 @@ function gripperHalfOpen() {
function gripperFullyOpen() {
console.log('sending fully open gripper command');
sendCommandWrist({type:'gripper', action:'fully_open'});
}
}
Loading

0 comments on commit a763551

Please sign in to comment.