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 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 ...
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.
i badly need solution to this, couldnt find any helpful resources