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
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: 0.0
y: 0.0
z: 0.0
[INFO] [1508329926.242838]: x: 0.0
y: 0.0
z: 0.0
[INFO] [1508329926.243337]: x: 0.0
y: 0.0
z: 0.0
...
The moveit_config file was download online and the launch file could run and show robot in Rviz. I don't know why it always show the error of package don't exist. I am a new learner to both moveit and python. Could anyone help me to check what's wrong with the code, this is not the result I want. I hope the streaming data of x,y,z could control robot movement.
Thank you in advance.
↧