def main()

in rostools/set_position.py [0:0]

• 38 lines of code
• 5 McCabe index (conditional complexity)

def main():
    arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
    arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_trajectory_client.wait_for_server()

    argi = 0
    while argi < len(sys.argv):
        arg = sys.argv[argi]
        argi += 1
        if arg == 'arm':
            print('Contacting move_group for arm...', file=sys.stderr)
            arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
            arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
            arm_trajectory_client.wait_for_server()

            joints = [
                'shoulder_pan_joint',
                'shoulder_lift_joint',
                'upperarm_roll_joint',
                'elbow_flex_joint',
                'forearm_roll_joint',
                'wrist_flex_joint',
                'wrist_roll_joint',
            ]
            pose = [float(a) for a in sys.argv[argi : argi+len(joints)]]
            argi += len(joints)
            result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                follow_goal = FollowJointTrajectoryGoal()
                follow_goal.trajectory = result.planned_trajectory.joint_trajectory
                print('sending trajectory to arm...', file=sys.stderr)
                self.arm_trajectory_client.send_goal(follow_goal)
                result = self.arm_trajectory_client.wait_for_result()
                print('arm followed trajectory %s' % result, file=sys.stderr)
            else:
                logger.warn('moveToJointPosition returned %s', result)
                return
        else:
            print('Unknown joint group %s' % arg, file=sys.stderr)
            return