Skip to content

Fix with latest ros3d #79

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
duwke wants to merge 2 commits into
base: kinetic-devel
Choose a base branch
from
Open

Fix with latest ros3d #79

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ac50c7e
Fix with latest ros3d, only show init or goal, use rospy
Aug 6, 2018
eeedb14
format fix
Aug 6, 2018
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
29 changes: 21 additions & 8 deletions rwt_moveit/nodes/interactive_moveit.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,10 @@
InteractiveMarkerFeedback)
from interactive_markers.interactive_marker_server import InteractiveMarkerServer
server = None
group_name = 'manipulator'
joint_states = None
joint_names = []
initial_joint_position = []


def makeInteractiveMarker(name, description):
Expand Down Expand Up @@ -49,7 +53,7 @@ def setColor(red, green, blue, alpha, marker):


def callback(req):
print req
rospy.logwarn(req)
return GetPositionIKResponse

def initial_callback(msg):
Expand Down Expand Up @@ -84,7 +88,7 @@ def feedback(feedback):
service = rospy.ServiceProxy('compute_ik', GetPositionIK)
request = GetPositionIKRequest()
request.ik_request.group_name = group_name
request.ik_request.timeout = rospy.Duration.from_sec(0.0001)
request.ik_request.timeout = rospy.Duration.from_sec(0.001)

# initial robot state
robot_state = RobotState()
Expand All @@ -106,10 +110,11 @@ def feedback(feedback):
pose_stamped.pose.orientation.w = feedback.pose.orientation.w

request.ik_request.pose_stamped = pose_stamped

response = service(request)
print response
# rospy.logwarn(response)
if len(response.solution.joint_state.position) != 0:
print "success"
rospy.loginfo("GetPositionIK succeeded")
msg = Float64MultiArray()
for i,joint_name in enumerate(response.solution.joint_state.name):
for j, name in enumerate(joint_names):
Expand All @@ -120,6 +125,8 @@ def feedback(feedback):
msg.layout.dim.append(dim)
msg.data.append(response.solution.joint_state.position[i])
pub.publish(msg)
else:
rospy.logfatal("GetPositionIK FAILED")

except rospy.ServiceException, e:
print "Service call failed: %s"%e
Expand Down Expand Up @@ -158,10 +165,16 @@ def feedback(feedback):
control_sphere = makeInteractiveMarkerControl(
interactive_marker, InteractiveMarkerControl.MOVE_ROTATE_3D)
marker = Marker()
marker.color.r = 0.2
marker.color.g = 0.3
marker.color.b = 0.7
marker.color.a = 0.5
if prefix == 'start':
marker.color.r = 0.2
marker.color.g = 0.2
marker.color.b = 0.7
marker.color.a = 0.5
else:
marker.color.r = 0.2
marker.color.g = 0.7
marker.color.b = 0.2
marker.color.a = 0.5

marker.type = Marker.SPHERE
marker.scale.x = 0.2
Expand Down
16 changes: 13 additions & 3 deletions rwt_moveit/nodes/link_group_publisher
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@

import roslib
import rospy
import sys
import xml.dom.minidom
from urdf_parser_py.urdf import URDF

def get_param(name, value=None):
private = "~%s" % name
Expand All @@ -19,6 +21,7 @@ def set_param(name, value):


def set_param():
robot = URDF.from_parameter_server()
description = get_param('robot_description_semantic')
group_description = xml.dom.minidom.parseString(description).getElementsByTagName('group')
effector_description = xml.dom.minidom.parseString(description).getElementsByTagName('end_effector')
Expand All @@ -28,13 +31,19 @@ def set_param():
for child in group_name.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
links.append(child.getAttribute('name'))
rospy.set_param('~' + name, links)
if child.getAttribute('name'):
links.append(child.getAttribute('name'))
if child.getAttribute('base_link') and child.getAttribute('tip_link') :
links.extend(robot.get_chain(child.getAttribute('base_link'),child.getAttribute('tip_link'), joints=True, links=False, fixed=False))
print('SET PARAM : ' + '/link_group/' + name, links)
if len(links) > 0:
rospy.set_param('/link_group/' + name, links)
for effector_name in effector_description:
if effector_name.nodeType is effector_name.TEXT_NODE:
continue
link_name = effector_name.getAttribute('parent_link')
group_name = effector_name.getAttribute('parent_group')
group_name = effector_name.getAttribute('group')
print('SET PARAM : ' + '/end_effector_link/' + group_name, link_name)
rospy.set_param('/end_effector_link/' + group_name, link_name)


Expand All @@ -44,3 +53,4 @@ if __name__ == '__main__':
rospy.init_node('link_group_publisher')
set_param()
except rospy.ROSInterruptException: pass
sys.exit(0)
25 changes: 17 additions & 8 deletions rwt_moveit/nodes/moveit_publisher.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
from sensor_msgs.msg import JointState
from moveit_msgs.msg import MoveGroupActionResult

plan_only = False

def joint_position_callback(joints):

global plan_only
Expand Down Expand Up @@ -81,20 +83,31 @@ def joint_position_callback(joints):
else:
plan_only = False

rospy.loginfo("send move_group_goal {}".format(move_group_goal))
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = client.get_result()
rospy.sleep(0.1)
rospy.loginfo("move_group_goal result {}".format(result.error_code.val))


except rospy.ROSInterruptException, e:
print "failed: %s"%e
rospy.logerr("failed: {}".format(e))


def moveit_callback(msg):
pub = rospy.Publisher('/update_joint_position', Float64MultiArray)
stock_pub = rospy.Publisher('/stock_joint_position', Float64MultiArray)
pub = rospy.Publisher('/update_joint_position', Float64MultiArray, queue_size=1)
stock_pub = rospy.Publisher('/stock_joint_position', Float64MultiArray, queue_size=1)

global plan_only

if plan_only:
global robot_trajectory
robot_trajectory = msg.result.planned_trajectory

r = rospy.Rate(10)
names = msg.result.planned_trajectory.joint_trajectory.joint_names
points = msg.result.planned_trajectory.joint_trajectory.points
Expand All @@ -113,10 +126,6 @@ def moveit_callback(msg):
stock_pub.publish(pos_msg)
r.sleep()

if plan_only:
global robot_trajectory
robot_trajectory = msg.result.planned_trajectory

def execute_callback(msg):
global robot_trajectory
rospy.wait_for_service('execute_kinematic_path')
Expand Down
5 changes: 2 additions & 3 deletions rwt_moveit/nodes/virtual_joint_state_publisher
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import roslib; roslib.load_manifest('joint_state_publisher')
import rospy
import wx
import xml.dom.minidom
from sensor_msgs.msg import JointState
from math import pi
Expand Down Expand Up @@ -145,11 +144,11 @@ class JointStatePublisher():
goal_value = goal_baseval * param.get('factor', 1) + param.get('offset', 0)

start_msg.name.append(str(name))
start_msg.position.append(value)
start_msg.position.append(start_value)
start_msg.velocity.append(0)

goal_msg.name.append(str(name))
goal_msg.position.append(value)
goal_msg.position.append(goal_value)
goal_msg.velocity.append(0)

self.start_pub.publish(start_msg)
Expand Down
55 changes: 27 additions & 28 deletions rwt_moveit/www/index.html
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,22 +4,32 @@
<meta charset="utf-8" />
<link rel="stylesheet"
type="text/css" href="/rwt_moveit/css/style.css" />
<!-- <link rel="stylesheet"
href="js/jquery-ui/themes/base/jquery.ui.all.css" />-->
<link rel="stylesheet"
href="http://code.jquery.com/mobile/1.3.2/jquery.mobile-1.3.2.min.css"
/>
<script src="http://code.jquery.com/jquery-1.9.1.min.js"></script>
<script type="text/javascript"
src="ros3djs/three.js"></script>
<script type="text/javascript"
src="ros3djs/ColladaLoader2.js"></script>
<script type="text/javascript"
src="http://cdn.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script type="text/javascript"
src="http://cdn.robotwebtools.org/roslibjs/current/roslib.js"></script>
<script type="text/javascript" src="http://cdn.robotwebtools.org/ros3djs/current/ros3d.js"></script>
<script type="text/javascript" src="rwt_moveit/js/basic.js"></script>

<script src="http://code.jquery.com/jquery-1.12.4.js" integrity="sha256-Qw82+bXyGq6MydymqBxNPYTaUXXq7c8v3CwiYwLLNXU=" crossorigin="anonymous"></script>
<!-- <script type="text/javascript" src="https://static.robotwebtools.org/threejs/r88/three.min.js"></script>
<script type="text/javascript" src="https://static.robotwebtools.org/ros3djs/0.18.0/ColladaLoader.js"></script>
<script type="text/javascript" src="https://static.robotwebtools.org/threejs/r88/STLLoader.js"></script>
<script type="text/javascript" src="https://static.robotwebtools.org/EventEmitter2/0.4.14/eventemitter2.min.js"></script>
<script type="text/javascript" src="http://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
<script type="text/javascript" src="http://static.robotwebtools.org/ros3djs/current/ros3d.js"></script> -->


<!-- use local copies so that if a cdn no longer works, the site still functions. See above for versions -->

<script type="text/javascript" src="/rwt_moveit/js/three.js"></script>
<script type="text/javascript" src="/rwt_moveit/js/ColladaLoader.js"></script>
<script type="text/javascript" src="/rwt_moveit/js/stlloader.js"></script>
<script type="text/javascript" src="/rwt_moveit/js/eventemitter2.js"></script>
<!-- roslibjs 0.20.0 -->
<script type="text/javascript" src="/rwt_moveit/js/roslib.js"></script>
<!-- this is https://github.com/RobotWebTools/ros3djs/commit/a816d6751e959fc0d5e41a78563d47febcd1bb5c
with pull request https://github.com/RobotWebTools/ros3djs/pull/232 to add 3d marker support -->
<script type="text/javascript" src="/rwt_moveit/js/ros3d.js"></script>

<script type="text/javascript" src="js/basic.js"></script>
</head>

<body onload="init()">
Expand All @@ -28,21 +38,10 @@ <h1>Web Robot Controller</h1>
<div id="urdf"></div>
<div id="control-content">
<table>
<tr><td></td><td>startState</td><td>goalState</td></tr>
<tr>
<td>
View
</td>
<td>
<input type="checkbox" name="start_state" id="start_state"/>
</td>
<td>
<input type="checkbox" name="goal_state" id="goal_state"/>
</td>
</tr>
<tr><td></td><td>startState</td><td>goalState</td></tr>
<tr>
<td>
Maniplation
Manipulation
</td>
<td>
<input type="radio" name="manip" id="manip" value="0"/>
Expand All @@ -52,11 +51,11 @@ <h1>Web Robot Controller</h1>
</td>
</tr>
</table>
<li>IM-Size
<li>IM-Size
<input type="number" name="number" id="im-size"
value="0.3" min="0" max="3" step="0.1"
onchange="im_size_callback()"/>
</li>
</li> -
<button id="init">Init</button>
<button id="preview">Preview</button>
<button id="plan">Plan</button>
Expand Down
Loading