Controlling an industrial servo motor through moveit

edit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

Edit: Thanks for the prompt reply.

So, I read about ros_control and modified the ros_control_boilerplate to have my railcontroller configured instead of the rrbot.
The code for my setup is here https://drive.google.com/drive/folder...

Here's the railrobot_controllers.yaml as defined in ros_control package:

# ros_control_boilerplate Settings -----------------------
# Settings for ros_control control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01

# Settings for ros_control hardware interface
hardware_interface:
   joints:
      - rail_joint

# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
   type:         joint_state_controller/JointStateController
   publish_rate: 50

# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
   type: position_controllers/JointTrajectoryController
   # These joints can likely just be copied from the hardware_interface list above
   joints:
      - rail_joint

# Individual Position Controllers ---------------------------------------
# Allows to send individual ROS msg of Float64 to each joint separately
rail_joint_position_controller:
   type: position_controllers/JointPositionController
   joint: joint1
   pid: {p: 100.0, i: 0.01, d: 10.0}

What I cant seem to figure out how the controller above defined here in ros_control_boilerplate/railrobot_control/config/railrobot_controllers.yaml ,will communicate with the follow joint trajectory action server that will in turn connect to moveit.

My understanding of ros_control and it's integration with moveit is quite amateur at the moment.I would appreciate if you can help me fill these missing pieces of knowledge.