Ask Your Question
1

Implement MoveIt! on Real Robot

asked 2014-09-15 22:55:02 -0500

cmeaclem gravatar image

updated 2014-09-17 14:43:48 -0500

Hi all,

Unfortunately I am really struggling with implementing MoveIt! on my custom hardware.

Basically I am stuck in connecting to an Action Client. I have been successful in using a driver package to control my motor drivers (RoboClaw). link from @bcharrow

Unfortunately in MoveIt am always greeted with:

[INFO] [1410916361.912676781]: MoveitSimpleControllerManager: Waiting for /full_ctrl/joint_trajectory_action to come up

[ERROR] [1410916366.912904732]: MoveitSimpleControllerManager: Action client not connected: /full_ctrl/joint_trajectory_action

[ INFO] [1410916371.938914542]: MoveitSimpleControllerManager: Waiting for /gripper_ctrl/joint_trajectory_action to come up

[ INFO] [1410916376.939103684]: MoveitSimpleControllerManager: Waiting for /gripper_ctrl/joint_trajectory_action to come up

[ERROR] [1410916381.939338320]: MoveitSimpleControllerManager: Action client not connected: /gripper_ctrl/joint_trajectory_action

[ INFO] [1410916381.957750506]: Returned 0 controllers in list

[ INFO] [1410916381.963234975]: Trajectory execution is managing controllers

My Action Client is based on this: link

Can anyone offer more of a step by step instruction on connecting my robot to MoveIt, i haven't found any such tutorial or etc such as: link1 link2

Cheers,

Chris.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
11

answered 2014-09-21 19:01:27 -0500

cmeaclem gravatar image

updated 2014-09-22 20:30:38 -0500

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.

edit flag offensive delete link more

Comments

Thanks @Rabe for the advice.

cmeaclem gravatar imagecmeaclem ( 2014-09-21 19:02:01 -0500 )edit
1

answered 2014-09-17 04:51:15 -0500

Rabe gravatar image

I guess you forgot to include the moveIt-Error message?

Anyways, to connect your robot you'll need to do the following stuf.

  • Create a proper urdf for your robot
  • Use the setup assistant to create the necessary files
  • Write a JointStatePublisher
  • Write an ActionServer waiting for FollowJointTrajectory-Messages

If you include the error-message, I might be able to help you more

edit flag offensive delete link more

Comments

Apologies, i have added the message now.

Thank you for the pointers, i will spend the day investigating the ActionServer.

I have already completed the first two steps but was previously trying to implement an ActionClient which is probably my problem.

cmeaclem gravatar imagecmeaclem ( 2014-09-17 14:45:40 -0500 )edit

I have been able to connect a basic ActionServer to MoveIt now. The next step must be to connect this server to my robot driver node. Would this be done by modifying the execute function of the server to communicate with the driver node?

cmeaclem gravatar imagecmeaclem ( 2014-09-17 16:34:06 -0500 )edit

AFAIK you don't need a robot driver node (at least for moveit). You can implement the code that controls your robot into the actionserver. If you need an explicit node that controls your robot, like for teleoperation or something like this, I still would think about doing the implentation within

Rabe gravatar imageRabe ( 2014-09-18 03:47:49 -0500 )edit

the action server, so that those two don't rely on each other, but this decision depends on your special case :)

Rabe gravatar imageRabe ( 2014-09-18 03:48:59 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

5 followers

Stats

Asked: 2014-09-15 22:55:02 -0500

Seen: 9,540 times

Last updated: Sep 22 '14