Implement MoveIt! on Real Robot

edit

Okay so for anyone out there with the same problem here is my solution:

I start from the end of the MoveIt Setup Assistant. Note that i use non-standard joint names.

Just as a caveat: This is not the best solution, simply one that worked for me. I hope it is helpful.

1. Setup your controllers.yaml such as the one below. This will include what controller is responsible for what actuator.
2. Implement an ActionServer for each controller. An example implementation is given below.
3. Modify the moveit_planning_execution.launch for your system. Again, my launch file is below.
4. Write a launch files to start the action server and moveit_planning_execution.launch.

The moveit_planning_execution.launch:

<launch>
  <!-- The planning and execution components of MoveIt! configured to run -->
  <!-- using the ROS-Industrial interface. -->

  <!-- Non-standard joint names:
       - Create a file <robot_moveit_dir>/config/joint_names.yaml
           controller_joint_names: [joint_1, joint_2, ... joint_N] 
       - Update with joint names for your robot (in order expected by rbt controller)
       - and uncomment the following line: -->
   <rosparam command="load" file="$(find <robot_moveit_dir>)/config/joint_names.yaml"/>

  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
  <!--  - if sim=false, a robot_ip argument is required -->
  <arg name="sim" default="false" />
  <arg name="robot_ip" unless="$(arg sim)" />

  <!-- load the robot_description parameter before launching ROS-I nodes -->
  <include file="$(find <robot_moveit_dir>)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <!-- run the robot simulator and action interface nodes -->
  <group if="$(arg sim)">
    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
  </group>

  <!-- publish the robot state (tf transforms) -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

  <include file="$(find <robot_moveit_dir>)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
  </include>

  <include file="$(find <robot_moveit_dir>)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
  </include>

  <include file="$(find <robot_moveit_dir>)/launch/default_warehouse_db.launch" />

</launch>

The controllers.yaml:

controller_list:
  - name: /full_ctrl
    action_ns: joint_trajectory_action
    type: FollowJointTrajectory
    default: true
    joints: [j_gripper_wrist1, j_wrist1_arm1, j_arm1_arm2, j_arm2_wrist2, j_wrist2_gripper2]

The example action server (full_action_server.cpp):

//http://clopema.felk.cvut.cz/redmine/projects/clopema/wiki/Sending_trajectory_to_the_controller
//http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29
//http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28GoalCallbackMethod%29
#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_;
  // NodeHandle instance must be created before this line. Otherwise strange error may occur.
  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()
  {
    // 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("/full_ctrl/joint_trajectory_action");
  ros::spin();

  return 0;
}

Regards,

Chris.