Controlling action server with Moveit

edit

Hi, I'm attempting to build ROS, and specifically moveit, support for a 5 jointed robot arm that until now has had no ROS integration. I've made a moveit config file that I can load correctly into rviz and plan paths with, using the demo.launch file, but when I attempt to plan and execute, my follow_joint_trajectory action server never receives anything. Right now the action server isn't connected to the robot itself and I'm just trying to check it can receive a trajectory at all. Here is my code for the server, however it initialises correctly and I'm pretty sure this isn't the problem.

#! /usr/bin/env python

import rospy

import actionlib

from control_msgs.msg import (
    FollowJointTrajectoryAction,
    FollowJointTrajectoryFeedback,
    FollowJointTrajectoryResult,
)

from trajectory_msgs.msg import (
    JointTrajectoryPoint
)

class JointTrajectoryActionServer(object):

    def __init__(self, controller_name):
        self._action_ns = controller_name + '/follow_joint_trajectory'
        self._as = actionlib.SimpleActionServer(
                self._action_ns,
                FollowJointTrajectoryAction,
                execute_cb=self.execute_cb,
                auto_start = False)
        self._action_name = rospy.get_name()
        self._as.start()
        self._feedback = FollowJointTrajectoryFeedback
        self._result = FollowJointTrajectoryResult
        rospy.loginfo('Successful init')

    def execute_cb(self, goal):
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        rospy.loginfo(trajectory_points)

if __name__ == '__main__':
    rospy.init_node('r12_interface')
    server = JointTrajectoryActionServer('r12_arm_controller')
    rospy.spin()

I also followed the moveit tutorials to make a controllers.yaml:

controller_list:
 - name: r12_arm_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints: - waist_joint 
           - shoulder_joint
           - elbow_joint
           - hand_joint
           - wrist_joint

and a moveit controller manager launch.xml

<launch>
 <rosparam file="$(find r12_moveit_config)/config/controllers.yaml"/>
 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
</launch>

I'm fairly lost where to go from here. Should I be launching something other than demo.launch?

[ INFO] [1533650558.379917234]: Combined planning and execution request received for MoveGroup action. 
Forwarding to planning and execution pipeline.
[ INFO] [1533650558.395341844]: Planning attempt 1 of at most 1
[ INFO] [1533650558.716230798]: Planner configuration 'ARM' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1533650558.810160782]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.810812725]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.811190164]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.811583198]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.837317543]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.838263460]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.840813138]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1533650558.886585428]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1533650558.887548330]: ParallelPlan::solve(): Solution found by one or more threads in 0.099139 seconds
[ INFO] [1533650558.888119729]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.888810442]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.889052261]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.889497329]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.889890463]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.890969620]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.891939037]: RRTConnect ...