Robotics StackExchange | Archived questions

Controlling action server with Moveit

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 followjointtrajectory 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: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.892332641]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1533650558.892451101]: ParallelPlan::solve(): Solution found by one or more threads in 0.004549 seconds
[ INFO] [1533650558.892717460]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.892796620]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1533650558.893849572]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1533650558.894081236]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1533650558.894227456]: ParallelPlan::solve(): Solution found by one or more threads in 0.001597 seconds
[ INFO] [1533650558.901687974]: SimpleSetup: Path simplification took 0.007098 seconds and changed from 3 to 2 states
[ INFO] [1533650559.009674142]: Fake execution of trajectory

This is the terminal output I get after plan and execute from rviz.

I'm running Ubuntu 18.02 and ROS melodic 1.14.2

Any help would be greatly appreciated!

Asked by AndreasHogstrand on 2018-08-07 09:10:54 UTC

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.

Asked by fvd on 2018-08-09 04:33:59 UTC

i badly need solution to this, couldnt find any helpful resources

Asked by s.vaichu on 2020-07-07 14:52:01 UTC

Answers

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?

Asked by fvd on 2018-08-09 04:39:19 UTC

Comments