int main(int argc, char** argv){ //init ROS node ros::init(argc, argv, "cartesian_ik_trajectory_executor"); IKTrajectoryExecutor ik_traj_exec = IKTrajectoryExecutor(); ROS_INFO("Waiting for cartesian trajectories to execute"); ros::spin(); return 0; }
int main(int argc, char** argv){ //init ROS node ros::init(argc, argv, "cartesian_ik_trajectory_executor"); ros::NodeHandle rh; planning_environment::CollisionModels* collisionModels; planning_models::KinematicState* kinematicState; collisionModels = new planning_environment::CollisionModels("robot_description"); kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel()); ros::AsyncSpinner spinner(1); spinner.start(); ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME); set_planning_scene_diff_client.waitForExistence(); ROS_INFO("Planning scene service is now available"); arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req; arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res; planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state); if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) { ROS_WARN("Can't get planning scene"); return -1; } ROS_INFO("Successfully set planning scene"); collisionModels->revertPlanningScene(kinematicState); collisionModels->setPlanningScene(planning_scene_res.planning_scene); IKTrajectoryExecutor ik_traj_exec = IKTrajectoryExecutor(); ROS_INFO("Waiting for cartesian trajectories to execute"); ros::spin(); return 0; }