Ask Your Question
0

Controlling action server with Moveit

asked 2018-08-07 09:10:54 -0500

AndreasHogstrand gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

As feedback to the question: I'd remove the last terminal output, since it's from demo.launch and your problem seems to be about connecting to the motor controllers. Can you post the launch file you start up your action server with instead? You probably also mean Ubuntu 18.04.

fvd gravatar imagefvd ( 2018-08-09 04:33:59 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-08-09 04:39:19 -0500

fvd gravatar image

updated 2018-08-09 04:39:47 -0500

Sadly I don't have much experience with this, but I would start by looking at how the panda and UR repositories did it.

Other debug directions to check:
- Are you sure that the topic the action advertises on is correct?
- Are the namespaces right?
- What is the launch file you use to start up your dummy follow_joint_trajectory server?
- What's the output of rostopic list after you start it up?
- Does the move_group node say anything helpful if you set the logger level to debug via rosrun rqt_logger_level rqt_logger_level?

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-08-07 09:10:54 -0500

Seen: 249 times

Last updated: Aug 09 '18