int main(int argc, char **argv) {
ros::init(argc, argv, "joint2_controller");
ros::NodeHandle nh;
ros::Duration half_sec(0.5);
// make sure service is available before attempting to proceed, else node will crash
bool service_ready = false;
while (!service_ready) {
service_ready = ros::service::exists("/gazebo/apply_joint_effort",true);
ROS_INFO("waiting for apply_joint_effort service");
half_sec.sleep();
}
ROS_INFO("apply_joint_effort service exists");
ros::ServiceClient set_trq_client =
nh.serviceClient<gazebo_msgs::ApplyJointEffort>("/gazebo/apply_joint_effort");
service_ready = false;
while (!service_ready) {
service_ready = ros::service::exists("/gazebo/get_joint_properties",true);
ROS_INFO("waiting for /gazebo/get_joint_properties service");
half_sec.sleep();
}
ROS_INFO("/gazebo/get_joint_properties service exists");
ros::ServiceClient get_jnt_state_client =
nh.serviceClient<gazebo_msgs::GetJointProperties>("/gazebo/get_joint_properties");
gazebo_msgs::ApplyJointEffort effort_cmd_srv_msg;
gazebo_msgs::GetJointProperties get_joint_state_srv_msg;
ros::Publisher trq_publisher = nh.advertise<std_msgs::Float64>("jnt2_trq", 1);
ros::Publisher vel_publisher = nh.advertise<std_msgs::Float64>("jnt2_vel", 1);
ros::Publisher pos_publisher = nh.advertise<std_msgs::Float64>("jnt2_pos", 1);
ros::Publisher joint_state_publisher = nh.advertise<sensor_msgs::JointState>("joint2_states", 1);
ros::Subscriber pos_cmd_subscriber = nh.subscribe("jnt2_pos_cmd",1,posCmdCB);
std_msgs::Float64 trq_msg;
std_msgs::Float64 q1_msg,q1dot_msg;
sensor_msgs::JointState joint_state_msg;
double q1, q1dot;
double dt = 0.01;
ros::Duration duration(dt);
ros::Rate rate_timer(1/dt);
effort_cmd_srv_msg.request.joint_name = "joint2";
effort_cmd_srv_msg.request.effort = 0.0;
effort_cmd_srv_msg.request.duration= duration;
get_joint_state_srv_msg.request.joint_name = "joint2";
//double q1_des = 1.0;
double q1_err;
double Kp = 50.0;
double Kv = 3;
double trq_cmd;
// set up the joint_state_msg fields to define a single joint,
// called joint1, and initial position and vel values of 0
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name.push_back("joint2");
joint_state_msg.position.push_back(0.0);
joint_state_msg.velocity.push_back(0.0);
while(ros::ok()) {
get_jnt_state_client.call(get_joint_state_srv_msg);
q1 = get_joint_state_srv_msg.response.position[0];
q1_msg.data = q1;
pos_publisher.publish(q1_msg);
q1dot = get_joint_state_srv_msg.response.rate[0];
q1dot_msg.data = q1dot;
vel_publisher.publish(q1dot_msg);
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.position[0] = q1;
joint_state_msg.velocity[0] = q1dot;
joint_state_publisher.publish(joint_state_msg);
//ROS_INFO("q1 = %f; q1dot = %f",q1,q1dot);
//watch for periodicity
q1_err= g_pos_cmd-q1;
if (q1_err>M_PI) {
q1_err -= 2*M_PI;
}
if (q1_err< -M_PI) {
q1_err += 2*M_PI;
}
trq_cmd = Kp*(q1_err)-Kv*q1dot;
//trq_cmd = sat(trq_cmd, 10.0); //saturate at 1 N-m
trq_msg.data = trq_cmd;
trq_publisher.publish(trq_msg);
// send torque command to Gazebo
effort_cmd_srv_msg.request.effort = trq_cmd;
set_trq_client.call(effort_cmd_srv_msg);
//make sure service call was successful
bool result = effort_cmd_srv_msg.response.success;
if (!result)
ROS_WARN("service call to apply_joint_effort failed!");
ros::spinOnce();
rate_timer.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "two_wheel_pos_minimal_controller");
ros::NodeHandle nh;
ros::Duration duration(dt); // duration for the torque to be exerted in the controller
// get initialization message of robot swarm from parameter server
std::string robot_model_name;
int robot_quantity;
bool get_name, get_quantity;
get_name = nh.getParam("/robot_model_name", robot_model_name);
get_quantity = nh.getParam("/robot_quantity", robot_quantity);
if (!(get_name && get_quantity))
return 0; // return if fail to get parameter
// resize global variables
g_left_wheel_poses.resize(robot_quantity);
g_left_wheel_vels.resize(robot_quantity);
g_left_wheel_poses_cmd.resize(robot_quantity);
g_right_wheel_poses.resize(robot_quantity);
g_right_wheel_vels.resize(robot_quantity);
g_right_wheel_poses_cmd.resize(robot_quantity);
// initialize a subscriber for "two_wheel_poses"
ros::Subscriber two_wheel_poses_subscriber = nh.subscribe("two_wheel_poses", 1, twoWheelPosesCallback);
// initialize a subscriber for "two_wheel_poses_cmd"
ros::Subscriber two_wheel_poses_cmd_subscriber =
nh.subscribe("two_wheel_poses_cmd", 1, twoWheelPosesCmdCallback);
while (!g_poses_callback_started) {
ros::Duration(0.5).sleep();
ros::spinOnce();
}
ROS_INFO("topic message from two_wheel_poses is ready");
// initialize a service client to apply joint effort
ros::ServiceClient set_wheel_torque_client = nh.serviceClient<gazebo_msgs::ApplyJointEffort>(
"/gazebo/apply_joint_effort");
gazebo_msgs::ApplyJointEffort set_wheel_torque_srv_msg; // service message
set_wheel_torque_srv_msg.request.effort = 0.0;
set_wheel_torque_srv_msg.request.duration = duration;
// make sure apply_joint_effort service is ready
ros::Duration half_sec(0.5);
bool service_ready = false;
while (!service_ready) {
service_ready = ros::service::exists("/gazebo/apply_joint_effort",true);
ROS_INFO("waiting for apply_joint_effort service");
half_sec.sleep();
}
ROS_INFO("apply_joint_effort service is ready");
// initialize a service to change g_kp & g_kv, service name is "two_wheel_kpkv"
ros::ServiceServer kpkv_service = nh.advertiseService("two_wheel_kpkv", twoWheelKpkvCallback);
// prepare control variables
ros::Rate rate_timer(1/dt);
double left_wheel_pos_err;
double left_wheel_torque;
double right_wheel_pos_err;
double right_wheel_torque;
std::string left_motor_name; // name of left motor
std::string right_motor_name; // name of right motor
// control loop
while (ros::ok()) {
// ROS_INFO("in the loop..."); // for debug
// apply joint effort loop
for (int i=0; i<robot_quantity; i++) {
// calculate the error
left_wheel_pos_err = g_left_wheel_poses_cmd[i] - g_left_wheel_poses[i];
right_wheel_pos_err = g_right_wheel_poses_cmd[i] - g_right_wheel_poses[i];
// do not watch for periodicity, that is, making error in (-M_PI, M_PI)
// because wheels are continuous rotating, angle could not always be in that range
// calculate the torque
left_wheel_torque = g_kp * left_wheel_pos_err - g_kv * g_left_wheel_vels[i];
right_wheel_torque = g_kp * right_wheel_pos_err - g_kv * g_right_wheel_vels[i];
// left_wheel_torque = 0.00001; // for debug
std::string s_index = intToString(i);
// actually cheat here, no better way to get the key words "left_motor" and "right_motor"
left_motor_name = robot_model_name + "_" + s_index + "::left_motor";
right_motor_name = robot_model_name + "_" + s_index + "::right_motor";
// set left wheel torque
set_wheel_torque_srv_msg.request.joint_name = left_motor_name;
set_wheel_torque_srv_msg.request.effort = left_wheel_torque;
// ROS_INFO_STREAM("left_wheel_torque " << left_wheel_torque); // for debug
set_wheel_torque_client.call(set_wheel_torque_srv_msg);
// set right wheel torque
set_wheel_torque_srv_msg.request.joint_name = right_motor_name;
set_wheel_torque_srv_msg.request.effort = right_wheel_torque;
// ROS_INFO_STREAM("right_wheel_torque " << right_wheel_torque); // for debug
set_wheel_torque_client.call(set_wheel_torque_srv_msg);
}
// frequency control
ros::spinOnce(); // update pos_cmd
rate_timer.sleep();
}
}
