Merge pull request #125 from cpaxton/feature/smartmove-constraints

Debugging info + data collection

costar_robot/costar_robot_manager/src/costar_robot/costar_arm.py

@@ -137,6 +137,7 @@ def __init__(self,
  self.plan_home_srv = self.make_service('PlanToHome',ServoToPose,self.plan_to_home_cb)
  self.smartmove = self.make_service('SmartMove',SmartMove,self.smart_move_cb)
  self.js_servo = self.make_service('ServoToJointState',ServoToJointState,self.servo_to_joints_cb)
+ self.js_plan = self.make_service('PlanToJointState',ServoToJointState,self.plan_to_joints_cb)
  self.smartmove_release_srv = self.make_service('SmartRelease',SmartMove,self.smartmove_release_cb)
  self.smartmove_grasp_srv = self.make_service('SmartGrasp',SmartMove,self.smartmove_grasp_cb)
  self.smartmove_grasp_srv = self.make_service('SmartPlace',SmartMove,self.smartmove_place_cb)
@@ -155,6 +156,7 @@ def __init__(self,
 
  self.status_pub = self.make_pub('DriverStatus',String,queue_size=1000)
  self.info_pub = self.make_pub('info',String,queue_size=1000)
+ self.object_pub = self.make_pub('SmartMove/object',String,queue_size=1000)
  self.display_pub = self.make_pub('display_trajectory',DisplayTrajectory,queue_size=1000)
 
  self.robot = URDF.from_parameter_server()
@@ -437,8 +439,9 @@ def send_trajectory(self,traj,stamp,acceleration=0.5,velocity=0.5,cartesian=Fals
  rospy.logerr("Function 'send_trajectory' not implemented for base class!")
  return "FAILURE -- running base class!"
 
- def info(self, msg):
+ def info(self, msg, object_name):
  self.info_pub.publish(data=msg)
+ self.object_pub.publish(data=object_name)
 
  '''
  Standard movement call.
@@ -514,16 +517,52 @@ def servo_to_home_cb(self,req):
  rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
  return 'FAILURE -- not in servo mode'
 
- '''
- Standard move call.
- Make a joint space move to a destination.
- '''
  def servo_to_joints_cb(self,req):
+ if self.driver_status == 'SERVO':
+
+ # Check acceleration and velocity limits
+ (acceleration, velocity) = self.check_req_speed_params(req)
+
+ stamp = self.acquire()
+
+ # inverse kinematics
+ traj = self.planner.getJointMove(req.target.position,
+ self.q0,
+ self.base_steps,
+ self.steps_per_meter,
+ self.steps_per_radians,
+ time_multiplier = (1./velocity),
+ percent_acc = acceleration,
+ use_joint_move = True,
+ table_frame = self.table_pose)
+
+ # Send command
+ if len(traj.points) > 0:
+ if stamp is not None:
+ rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
+ res = self.send_trajectory(traj,stamp,acceleration,velocity,cartesian=False)
+ self.release()
+ else:
+ res = 'FAILURE -- could not preempt current arm control.'
+ return res
+ else:
+ rospy.logerr('SIMPLE DRIVER -- IK failed')
+ return 'FAILURE -- no trajectory points'
+ else:
+ rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
+ return 'FAILURE -- not in servo mode'
+
+ def plan_to_joints_cb(self,req):
 
  if self.driver_status == 'SERVO':
  # Check acceleration and velocity limits
  (acceleration, velocity) = self.check_req_speed_params(req) 
- return 'FAILURE -- not yet implemented!'
+ stamp = self.acquire()
+
+ # Send command
+ pt = JointTrajectoryPoint()
+ (code,res) = self.planner.getPlan(q_goal=req.target.position,q=self.q0)
+ return self.send_and_publish_planning_result(res,stamp,acceleration,velocity)
 
  else:
  rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
@@ -1006,17 +1045,17 @@ def smartmove_multipurpose_gripper(self,
  if dist > 2 * backup_dist:
  rospy.logwarn("Backoff failed for pose %i: distance was %f vs %f"%(sequence_number,dist,2*backup_dist))
  continue
- self.info("appoach_%s"%obj)
+ self.info("appoach_%s"%obj, obj)
  msg = self.send_and_publish_planning_result(res,stamp,acceleration,velocity)
  rospy.sleep(0.1)
 
  if msg[0:7] == 'SUCCESS':
- self.info("move_to_grasp_%s"%obj)
+ self.info("move_to_grasp_%s"%obj, obj)
  msg = self.send_and_publish_planning_result(res2,stamp,acceleration,velocity)
  rospy.sleep(0.1)
 
  if msg[0:7] == 'SUCCESS':
- self.info("take_%s"%obj)
+ self.info("take_%s"%obj, obj)
  gripper_function(obj)
 
  traj = res2.planned_trajectory.joint_trajectory
@@ -1030,7 +1069,7 @@ def smartmove_multipurpose_gripper(self,
  traj.points[0].positions = self.q0
  traj.points[-1].velocities = [0.]*len(self.q0)
 
- self.info("backoff_from_%s"%obj)
+ self.info("backoff_from_%s"%obj, obj)
  msg = self.send_and_publish_planning_result(res2,stamp,acceleration,velocity)
  return msg
  else:
@@ -1103,7 +1142,7 @@ def smartmove_release_cb(self, req):
  stamp = self.acquire()
  list_of_waypoints = self.query(req, True)
  if len(list_of_waypoints) == 0:
- return "FAILURE -- no suitable points found"
+ return "FAILURE -- no suitable waypoints found for release"
  distance = req.backoff
  return self.smartmove_release(stamp, list_of_waypoints, distance, req.vel, req.accel)
 
@@ -1114,7 +1153,7 @@ def smartmove_grasp_cb(self, req):
  stamp = self.acquire()
  list_of_waypoints = self.query(req, True)
  if len(list_of_waypoints) == 0:
- return "FAILURE -- no suitable points found"
+ return "FAILURE -- no suitable waypoints found for grasp"
  distance = req.backoff
  return self.smartmove_grasp(stamp, list_of_waypoints, distance, req.vel, req.accel)