send realtime data to moveit teleoperate robot

asked 2017-10-18 21:05:12 -0500

huang gravatar image

Hello, I have designed a master device to control robot. I want to use moveit to do motion planning. I want to send three dof. position data to robot end-effector . I have write a talker node to send the data, and I need another listener code to receive this data and send to moveit. I attached my code here.

#!/usr/bin/env python
import sys
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import rospy
from geometry_msgs.msg import Twist

vx = 0
vy = 0
vz = 0

def callback(data):
rospy.loginfo(data.linear)
global vx,vy,vz
vx = data.linear.x
vy = data.linear.y
vz = data.linear.z

def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("fi", Twist, callback)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher(
    '/move_group/display_planned_path',
    moveit_msgs.msg.DisplayTrajectory)
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
## Planning to a Pose goal:
print "============ Generating plan 1"
current_pose = group.get_current_pose().pose
rospy.loginfo('current_pose: {}'.format(current_pose))
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = current_pose.orientation.w
pose_target.orientation.x = current_pose.orientation.x
pose_target.orientation.y = current_pose.orientation.y
pose_target.orientation.z = current_pose.orientation.z
pose_target.position.x = current_pose.position.x + vx/10
pose_target.position.y = current_pose.position.y + vy/10
pose_target.position.z = current_pose.position.z + vz/10
group.set_pose_target(pose_target)
plan1 = group.plan()
group.go(wait=True)
moveit_commander.roscpp_shutdown()
print "stop"
rospy.spin()

if __name__ == '__main__':
print "Program start listerning..."
main()

I hope the moveit could read my read-time data of vx, vy and vz, then send these to current position. Then I could achieve remote control by my device. Now the running result is:

 Program start listerning...
[ INFO] [1508329377.270614560]: Loading robot model 'abb_irb120_3_58'...
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.306347223]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/collision/base_link.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.326927698]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/visual/base_link.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.349068945]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/collision/link_1.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
...
      [ WARN] [1508329377.983231280]: No geometry is associated to any robot links
       [ WARN] [1508329377.983386064]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
        Traceback (most recent call last):
          File "moveit_listener3.py", line 58, in <module>
            main()
          File "moveit_listener3.py", line 27, in main
            group = moveit_commander.MoveGroupCommander("manipulator")
          File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 51, in __init__
            self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description)
        RuntimeError: Unable to connect to move_group action server 'move_group' within allotted time (5s)
       [INFO] [1508329926.239926]: x ...
(more)
edit retag flag offensive close merge delete