//! Initialize the action client and wait for action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_r_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
traj_client_l_ = new TrajClient("l_arm_controller/joint_trajectory_action", true);
// wait for action server to come up
while(!traj_client_l_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
// wait for action server to come up
while(!traj_client_r_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
IKTrajectoryExecutor()
{
//tell the joint trajectory action client that we want
//to spin a thread by default
action_client = new TrajClient("joint_trajectory_action", true);
//wait for the action server to come up
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
//register a service to input desired Cartesian trajectories
service = node.advertiseService("execute_cartesian_ik_trajectory",
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
//have to specify the order of the joints we're sending in our
//joint trajectory goal, even if they're already on the param server
goal.trajectory.joint_names.push_back("shoulder_pan_joint");
goal.trajectory.joint_names.push_back("shoulder_lift_joint");
goal.trajectory.joint_names.push_back("elbow_joint");
goal.trajectory.joint_names.push_back("wrist_1_joint");
goal.trajectory.joint_names.push_back("wrist_2_joint");
goal.trajectory.joint_names.push_back("wrist_3_joint");
}
IKTrajectoryExecutor(){
//create a client function for the IK service
ik_client = node.serviceClient<kinematics_msgs::GetPositionIK>
(ARM_IK_NAME, true);
//wait for the various services to be ready
ROS_INFO("Waiting for services to be ready");
ros::service::waitForService(ARM_IK_NAME);
ROS_INFO("Services ready");
//tell the joint trajectory action client that we want
//to spin a thread by default
action_client = new TrajClient("l_arm_controller/joint_trajectory_action", true);
//wait for the action server to come up
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
//register a service to input desired Cartesian trajectories
service = node.advertiseService("left_execute_cartesian_ik_trajectory",
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
//have to specify the order of the joints we're sending in our
//joint trajectory goal, even if they're already on the param server
goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
}
// Initialize the action client and wait for the action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_elbow_flex_ = new TrajClient("r_elbow_flex_controller/follow_joint_trajectory", true);
traj_client_torso_ = new TrajClient("/torso_lift_controller/follow_joint_trajectory", true);
// wait for action server to come up
while (!traj_client_elbow_flex_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
}
while (!traj_client_torso_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the r_elbow_flex_controller/follow_joint_trajectory server");
}
ROS_INFO("Works for now");
}
//! Initialize the action client and wait for action server to come up
MoveInJoints()
{
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/move_iri_wam", true);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_iri_wam server");
}
}
//! Initialize the action client and wait for action server to come up
RobotArm():srv_name_("/iri_wam/iri_wam_controller/joints_move") {
client = n.serviceClient<iri_common_drivers_msgs::QueryJointsMovement>(this->srv_name_.c_str());
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/iri_wam/iri_wam_controller/follow_joint_trajectory", true);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
//! Initialize the action client and wait for action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("/wam_wrapper/DMPTracker", true);
//initialize the topic to send new goals
pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/wam_wrapper/DMPTrackerNewGoal", 1);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
void init (ros::NodeHandle* nh) {
ik_client = nh->serviceClient<moveit_msgs::GetPositionIK>
(ARM_IK_NAME, true);
ROS_INFO("Waiting for services to be ready");
ros::service::waitForService(ARM_IK_NAME);
ROS_INFO("Services ready");
// action_client = new TrajClient("joint_trajectory_action", true);
action_client = new TrajClient("follow_joint_trajectory", true);
while(ros::ok() && !action_client->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
}
// service = node.advertiseService("execute_cartesian_ik_trajectory",
// &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
const std::string input_topic = "execute_cartesian_ik_trajectory";
joint_states_sub = nh->subscribe( "joint_states", 1,
&IKTrajectoryExecutor::set_current_joint_angles, this);
ros::Duration(1.0).sleep();
trajectory_sub = nh->subscribe( input_topic, 1,
&IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this);
ros::spinOnce();
goal.trajectory.joint_names.push_back("shoulder_pan_joint");
goal.trajectory.joint_names.push_back("shoulder_lift_joint");
goal.trajectory.joint_names.push_back("elbow_joint");
goal.trajectory.joint_names.push_back("wrist_1_joint");
goal.trajectory.joint_names.push_back("wrist_2_joint");
goal.trajectory.joint_names.push_back("wrist_3_joint");
}
