My purpose: In the Move Group Python Interface section in the Moveit it tutorials(kinetic version), it shows we can plan a motion for a move group to a desired pose for the end-effector and excute it as shown is the snippet of code below: pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.w = 1.0 pose_goal.position.x = 0.4 pose_goal.position.y = 0.1 pose_goal.position.z = 0.4 group.set_pose_target(pose_goal)
In this example code available on moveit site, however, we can only plan one pose (instead of a number of successive poses which can form a smooth line) at a time. So I've got an idea that I could augmented this piece of code with a subscriber which listens to a ROS sensor sending poses to move group in this code then plan and execute those desired pose. Ideally, in this way, I can perform a smooth trajectory with all the poses together.
My first question is whether what I am planning to do feasible and practical?
If so, I've got some other questions while working on it. I am using a leap motion sensor as a publisher (publishing hand positions) in the ROS structure. As I said I would build a subscriber in the Move Group Python Interface code to keep receiving new poses then plan and execute in time. To get the job done: First,I created a package named leap_moveit with the following depedencies: catkin_create_pkg leap_moveit rospy rospack moveit_commander moveit_msgs geometry_msgs
Secondly,I put the modified Move Group Python Interface code(the main difference is there is a subscriber to obtain new desired poses in this code). The subscriber is: leap_sub = rospy.Subscriber('leapmotion/data', leapros, go_to_joint_state)
Thirdly,I updated CMakeLists.txt and package.xml accordingly and created a launch file in the launch directory of my leap_moveit pkg.
Finally, when I run roslaunch leap_moveit leapmoveit.launch to try out the code, I throws the following errors. Can anyone help?
mario@mario:~$ roslaunch leap_moveit leapmoveit.launch
... logging to /home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/roslaunch-mario-30026.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/mario/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.
started roslaunch server http://localhost:32869/
SUMMARY
========
PARAMETERS
* /rosdistro: kinetic
* /rosversion: 1.12.14
NODES
/
leapmoveit_node (leap_moveit/move_group_python_interface_leap.py)
auto-starting new master
process[master]: started with pid [30043]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to fa712ea8-1ab7-11e9-a98a-94c6911255ab
process[rosout-1]: started with pid [30063]
started core service [/rosout]
process[leapmoveit_node-2]: started with pid [30069]
Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info
[ERROR] [1547771310.996982652]: Robot model parameter not found! Did you remap 'robot_description'?
Traceback (most recent call last):
File "/home/mario/catkin_ws/src/leap_moveit/src/move_
group_python_interface_leap.py", line 223, in <module>
main()
File "/home/mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py", line 202, in main
tutorial = MoveGroupPythonIntefaceTutorial()
File "/home/mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py", line 57, in __init__
robot = moveit_commander.RobotCommander()
File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_commander/robot.py", line 151, in __init__
self._r = _moveit_robot_interface.RobotInterface(robot_description, ns)
RuntimeError: RobotInterfacePython: invalid robot model
[leapmoveit_node-2] process has died [pid 30069, exit code 1, cmd /home/
mario/catkin_ws/src/leap_moveit/src/move_group_python_interface_leap.py __name:=leapmoveit_node __log:=/home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/leapmoveit_node-2.log].
log file: /home/mario/.ros/log/fa712ea8-1ab7-11e9-a98a-94c6911255ab/leapmoveit_node-2*.log
Thank a million for your great support!!! And my python code in src in my leap_moveit package is shown below:
#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
###
from leap_motion.msg import leap
from leap_motion.msg import leapros
## END_SUB_TUTORIAL
def all_close(goal, actual, tolerance):
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > toler
ance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
class MoveGroupPythonIntefaceTutorial(object):
"""MoveGroupPythonIntefaceTutorial"""
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_leap',
anonymous=True)'''
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
leap_sub = rospy.Subscriber('leapmotion/data', leapros, go_to_joint_state)
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
###
self.leap_sub = leap_sub
self.go_to_joint_state = go_to_joint_state
def go_to_pose_goal(self, msg):
## We can plan a motion for this group to a desired pose for the
## end-effector:
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = msg.palmpos.y/1000
pose_goal.position.y = msg.palmpos.x/1000
pose_goal.position.z = msg.palmpos.y/1000
group.set_pose_target(pose_goal)
## Now, we call the planner to compute the plan and execute it.
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
#group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()
## END_SUB_TUTORIAL
def execute_plan(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group
def main():
try:
tutorial = MoveGroupPythonIntefaceTutorial()
tutorial.go_to_pose_goal()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == '__main__':
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_leap',
anonymous=True)
main()
rospy.spin()
Originally posted by stevensu1838 on ROS Answers with karma: 27 on 2019-01-17
Post score: 1
Original comments
Comment by jayess on 2019-01-17:
Can you please update your question with all of the comments removed? Your code is unnecessarily long with them in and difficult to read
Comment by stevensu1838 on 2019-01-18:
Hi buddy, updated. Basically, what I've done is I added a subscriber in the Move Group Python Interface example code.
Comment by stevensu1838 on 2019-01-18:
But it doesn't work as expected. Is my idea of creating a subscriber wrong? Can you do me a favour?
Comment by vk on 2019-07-14:
hi did yo succeed in Create a subscriber and executing a continuous path?