Skip to content
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

fix for robot defined with link chain #66

Open
wants to merge 5 commits into
base: kinetic-devel
Choose a base branch
from
Open

fix for robot defined with link chain #66

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e182980
import wx is not requried, and obsoluted in kinetic
k-okada Nov 12, 2017
64863dc
manipulators defined using chain need URDF informatoin
k-okada Nov 12, 2017
2d60606
initialized global variables
k-okada Nov 12, 2017
55aff1f
display message via ros logger
k-okada Nov 12, 2017
55aa6a7
[moveit_publisher.py] add more debug message
k-okada Nov 12, 2017
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
10 changes: 7 additions & 3 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 @@ -107,9 +111,9 @@ def feedback(feedback):

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 Down
15 changes: 12 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,18 @@ 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)
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 +52,4 @@ if __name__ == '__main__':
rospy.init_node('link_group_publisher')
set_param()
except rospy.ROSInterruptException: pass
sys.exit(0)
6 changes: 4 additions & 2 deletions rwt_moveit/nodes/moveit_publisher.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,14 @@ 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))
result = client.wait_for_result(rospy.Duration.from_sec(5.0))
rospy.loginfo("move_group_goal result {}".format(result))


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


def moveit_callback(msg):
Expand Down
1 change: 0 additions & 1 deletion 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