Controlling an industrial servo motor through moveit
I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.
controller_list:
- name: "arm_controller"
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
- name: "rail_controller"
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- rail_joint
As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file
#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_;
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()
{
ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
// 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("/rail_controller/follow_joint_trajectory");
ros::spin();
return 0;
}
What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.
Edit: Thanks for the prompt reply.
So, I read about ros_control and modified the ros_control_boilerplate to have my railcontroller configured instead of the rrbot.
The code for my setup is here https://drive.google.com/drive/folder...
Here's the railrobot_controllers.yaml as defined in ros_control package:
# ros_control_boilerplate Settings -----------------------
# Settings for ros_control control loop
generic_hw_control_loop:
loop_hz: 300
cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
joints:
- rail_joint
# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
type: position_controllers/JointTrajectoryController
# These joints can likely just be copied from the hardware_interface list above
joints:
- rail_joint
# Individual Position Controllers ---------------------------------------
# Allows to send individual ROS msg of Float64 to each joint separately
rail_joint_position_controller:
type: position_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 0.01, d: 10.0}
What I cant seem to figure out how the controller above defined here in ros_control_boilerplate/railrobot_control/config/railrobot_controllers.yaml ,will communicate with the follow joint trajectory action server that will in turn connect to moveit.
My understanding of ros_control and it's integration with moveit is quite amateur at the moment.I would appreciate if you can help me fill these missing pieces of knowledge.
Please don't post answers unless you are actually answering your own question. For everything else, use comments or update your original question. This is not a regular forum (with multiple posts following each other).
I've merged your answer into your OP.