Permalink
Browse files

Merge pull request #14 from tork-a/add_ur5_tests

Add ur5 tests
  • Loading branch information...
7675t committed Jul 10, 2018
2 parents c72fbd9 + 3b94d3a commit c775b63038d233dc04ceb7ae2e24f35e5369d77c
@@ -113,7 +113,7 @@ if(CATKIN_ENABLE_TESTING)
# roslaunch_add_file_check(${LAUNCH_FILE})
# endforeach()
# add_rostest(test/tra1_jog.test ARGS use_rviz:=false)
# add_rostest(test/baxter_jog.test ARGS use_rviz:=false)
add_rostest(test/ur5_jog.test ARGS use_gui:=false)
add_rostest(test/nextage_jog.test ARGS use_rviz:=false)
# add_rostest(test/baxter_jog.test ARGS use_rviz:=false)
endif()
@@ -154,12 +154,10 @@ void JogJointNode::jog_joint_cb(jog_msgs::JogJointConstPtr msg)
ROS_ERROR("Size mismatch of joint_names and deltas");
return;
}
ROS_INFO_STREAM("msg:" << *msg);
// Update only if the stamp is older than last_stamp_ + time_from_start
if (msg->header.stamp > last_stamp_ + ros::Duration(time_from_start_))
{
ROS_INFO("ref joint state updated");
ROS_INFO("start joint state updated");
// Update reference joint_state
joint_state_.name.clear();
joint_state_.position.clear();
@@ -184,10 +182,8 @@ void JogJointNode::jog_joint_cb(jog_msgs::JogJointConstPtr msg)
auto controller_name = it->first;
auto joint_names = it->second.joints;
trajectory_msgs::JointTrajectory traj;
trajectory_msgs::JointTrajectoryPoint point;
point.positions.resize(joint_names.size());
point.velocities.resize(joint_names.size());
point.accelerations.resize(joint_names.size());
point.time_from_start = ros::Duration(time_from_start_);
for (int i=0; i<joint_names.size(); i++)
@@ -208,35 +204,30 @@ void JogJointNode::jog_joint_cb(jog_msgs::JogJointConstPtr msg)
ROS_ERROR_STREAM("Cannot find joint " << joint_names[i] << " in joint_states_");
continue;
}
// Update reference joint position
// Update start joint position
joint_state_.position[state_index] += msg->deltas[jog_index];
point.positions[i] = joint_state_.position[state_index];
point.velocities[i] = 0.0;
point.accelerations[i] = 0.0;
// Fill joint trajectory joint_names and positions
traj.joint_names.push_back(joint_names[i]);
point.positions.push_back(joint_state_.position[state_index]);
point.velocities.push_back(0.0);
point.accelerations.push_back(0.0);
}
// Fill joint trajectory members
traj.header.stamp = ros::Time::now();
traj.header.frame_id = "base_link";
traj.points.push_back(point);
if (use_action_)
{
control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory.header.stamp = ros::Time::now();
goal.trajectory.header.frame_id = "base_link";
goal.trajectory.joint_names = joint_names;
goal.trajectory.points.push_back(point);
goal.trajectory = traj;
traj_clients_[controller_name]->sendGoal(goal);
}
else
{
trajectory_msgs::JointTrajectory traj;
traj.header.stamp = ros::Time::now();
traj.header.frame_id = "base_link";
traj.joint_names = joint_names;
traj.points.push_back(point);
traj_pubs_[controller_name].publish(traj);
}
}
// Publish reference joint state for debug
// Publish start joint state for debug
joint_state_pub_.publish(joint_state_);
}
@@ -0,0 +1,176 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import actionlib
from control_msgs.msg import (
FollowJointTrajectoryAction,
FollowJointTrajectoryGoal,
)
import math
import rospy
import sys
from trajectory_msgs.msg import (
JointTrajectoryPoint,
)
import sys
from jog_msgs.msg import JogFrame
import rospy
import rostest
from sensor_msgs.msg import JointState
import tf
import unittest
class TestJogFrameNode(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_jog_frame_node')
def setUp(self):
# Get parameters
self.controller_name = rospy.get_param('~controller_name', 'arm_controller')
self.action_name = rospy.get_param('~action_name', 'follow_joint_trajectory')
self.joint_names = rospy.get_param('~joint_names', [])
self.home_positions = rospy.get_param('~home_positions', [])
self.frame_id = rospy.get_param('~frame_id', 'base_link')
self.group_name = rospy.get_param('~group_name', 'manipulator')
self.link_name = rospy.get_param('~link_name', 'ee_link')
self.linear_delta = rospy.get_param('~linear_delta', 0.005)
self.angular_delta = rospy.get_param('~angular_delta', 0.01)
# TF listener
self.listener = tf.TransformListener()
# Publishers
self.pub = rospy.Publisher('jog_frame', JogFrame, queue_size=10)
# Actionlib
self.client = actionlib.SimpleActionClient(
self.controller_name + '/' + self.action_name, FollowJointTrajectoryAction)
self.client.wait_for_server()
# Move to home position
self.move_to_home()
def move_to_home(self):
goal = FollowJointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.Time.now()
goal_time_tolerance = rospy.Time(0.1)
goal.goal_time_tolerance = rospy.Time(0.1)
goal.trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint()
point.positions = self.home_positions
point.time_from_start = rospy.Duration(1.0)
goal.trajectory.points.append(point)
self.client.send_goal(goal)
self.assertTrue(self.client.wait_for_result(timeout=rospy.Duration(5.0)))
rospy.sleep(1.0)
def test_a_jog_with_linear_delta(self):
'''Test to jog a command with linear delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.linear_delta.x = self.linear_delta
jog.linear_delta.y = self.linear_delta
jog.linear_delta.z = self.linear_delta
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is jogged by delta
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta, delta=0.0001)
# Check if the rotaion is not changed
rot_diff = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_inverse(start_rot), rot)
for i in range(4):
self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
def test_a_jog_with_angular_delta(self):
'''Test to jog a command with angular delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.angular_delta.x = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.y = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.z = self.angular_delta / math.sqrt(3.0)
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is not changed
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001)
# Check if the rotaion is jogged by delta
jog_q = tf.transformations.quaternion_about_axis(self.angular_delta, [1,1,1])
ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot)
for i in range(4):
self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
def test_ten_jogs_with_linear_delta(self):
'''Test to jog ten commands with linear delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
for i in range(10):
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.linear_delta.x = self.linear_delta
jog.linear_delta.y = self.linear_delta
jog.linear_delta.z = self.linear_delta
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is jogged by delta
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i] + self.linear_delta*10, delta=0.0001)
# Check if the rotaion is not changed
for i in range(4):
self.assertAlmostEqual(start_rot[i], rot[i], delta=0.0001)
def test_ten_jogs_with_angular_delta(self):
'''Test to jog ten commands with angular delta'''
# Get current posture of link_name
(start_pos, start_rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
for i in range(10):
# Create jog frame command
jog = JogFrame()
jog.header.stamp = rospy.Time.now()
jog.header.frame_id = self.frame_id
jog.group_name = self.group_name
jog.link_name = self.link_name
jog.angular_delta.x = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.y = self.angular_delta / math.sqrt(3.0)
jog.angular_delta.z = self.angular_delta / math.sqrt(3.0)
self.pub.publish(jog)
rospy.sleep(3.0)
(pos, rot) = self.listener.lookupTransform(
self.frame_id, self.link_name, rospy.Time(0))
# Check if the position is not changed
for i in range(3):
self.assertAlmostEqual(pos[i], start_pos[i], delta=0.0001)
# Check if the rotaion is jogged by delta
jog_q = tf.transformations.quaternion_about_axis(self.angular_delta*10, [1,1,1])
ans_rot = tf.transformations.quaternion_multiply(jog_q, start_rot)
for i in range(4):
self.assertAlmostEqual(rot[i], ans_rot[i], delta=0.0001)
if __name__ == '__main__':
rostest.rosrun('jog_frame', 'test_jog_frame_node', TestJogFrameNode)
Oops, something went wrong.

0 comments on commit c775b63

Please sign in to comment.