I'm customising the **moveit_cartesian_demo.py** script from the *ROS by example V2* book. I'm subscribing to a **/target_pose** topic from which I get poses which I am trying to append to a list and later act upon in my code.
My issue is my **waypoints** list is empty after my subscriber callback function executes as my code below shows:
#!/usr/bin/env python
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
# from geometry_msgs.msg import Pose
from copy import deepcopy
from geometry_msgs.msg import PoseStamped, Pose, PointStamped
waypoints = null
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
# right_arm.set_pose_reference_frame('base_footprint')
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "home" configuration stored in the SRDF file
right_arm.set_named_target('home')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the self.waypoints list
self.waypoints = []
self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
rospy.loginfo(self.waypoints)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the self.waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
self.waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Move normally back to the 'up' position
right_arm.set_named_target('up')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def update_target_pose(self, data):
self.waypoints.append(data.pose)
if __name__ == "__main__":
try:
MoveItDemo()
except rospy.ROSInterruptException:
pass
Though the *target_pose* topic is publishing pose data, I've been starring at this for a while and I'm not sure why the waypoints list is empty.
I would appreciate your help.
↧