// Select which mode to use from launch file. Fixed numbers for LASA/BOXY robot or the Vision tracking. void initialize() { std::string ad; ros::NodeHandle _nh("~"); _nh.getParam("action_mode", ad); if(ad == "lasa_fixed") { action_mode = ACTION_LASA_FIXED; } else if(ad == "boxy_fixed") { action_mode = ACTION_BOXY_FIXED; } else if(ad == "vision") { action_mode = ACTION_VISION; } else { ROS_ERROR_STREAM("Unrecognized action type. Must be one of 'lasa_fixed', 'boxy_fixed', 'vision'. Found '"<<ad<<"'"); throw; } _nh.getParam("world_frame", world_frame); _nh.getParam("model_base_path", base_path); _nh.getParam("use_boxy_tool", use_boxy_tool); _nh.getParam("path_matlab_plot", path_matlab_plot); if (use_boxy_tool) ROS_INFO("Using Boxy Tool adding extra Rotation"); ROS_INFO_STREAM("Selected Action Mode: " << ad); // Useful for running this in simulation. WaitForForces() would never return if ran with simulator. if(!_nh.getParam("wait_for_force", bWaitForForces)) { ROS_INFO_STREAM("Set the Waiting for forces flag"); bWaitForForces = true; } }
int main(int argc, char** argv) { //ROS setup ros::init(argc, argv, "read_from_camera_node"); ros::NodeHandle _nh("~"); ROS_DEBUG("read_from_camera_node Initialized"); ros::Subscriber left_image_subscriber = _nh.subscribe("left_image",1,left_image_callback); ros::Subscriber right_image_subscriber = _nh.subscribe("right_image",1,right_image_callback); left_image_publisher = _nh.advertise<sensor_msgs::Image>("capture_left_frame",1); right_image_publisher = _nh.advertise<sensor_msgs::Image>("capture_right_frame",1); ros::ServiceServer next_frame_service = _nh.advertiseService("next_frame_service",next_frame); fetch_next_right_image = false; fetch_next_left_image = false; ROS_DEBUG("read_from_camera_node Ready to process requests"); ros::spin(); return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "joint_to_cart"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); mRobot = new RTKRobotArm(true); if(!mRobot->initialize(_nh)) { ROS_ERROR("Error while loading robot"); return 1; } if(!parseParams(_nh)) { ROS_ERROR("Errors while parsing arguments."); return 1; } numdof = mRobot->numdof; read_torque.resize(numdof); read_jpos.resize(numdof); ee_ft.resize(6); pub_pose = nh.advertise<geometry_msgs::PoseStamped>(output_cart_pose, 3); pub_ft = nh.advertise<geometry_msgs::WrenchStamped>(output_cart_ft, 3); ros::Subscriber sub = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 10, jointStateCallback,ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>("/right_arm_ft_sensor/wrench", 10, sensorFTCallback, ros::TransportHints().tcpNoDelay()); ROS_INFO("Node started"); ros::spin(); return 0; }
void LoadThread() { if(!this->husky){ ROS_FATAL_STREAM("Husky plugin requires a husky as its parent."); return; } ros::NodeHandle _nh("husky"); subscriber = _nh.subscribe("diff_wrench", 1000, &HuskySimulator::HuskyStateCallback, this); this->_updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&HuskySimulator::OnUpdate, this, _1)); ROS_INFO("Husky plugin for ROS successfully loaded :)"); // Check for new messages // ros::spin(); while(ros::ok()){ ros::spinOnce(); } }
int main(int argc, char** argv) { ros::init(argc, argv, "cart_to_joint"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); if(!parseParams(_nh)) { ROS_ERROR("Errors while parsing arguments."); return 1; } // Initialize robot with/without orientation (variable from launch file) mRobot = new RTKRobotArm(bOrientCtrl); if(!mRobot->initialize(_nh)) { ROS_ERROR("Error while loading robot"); return 1; } numdof = mRobot->numdof; des_ee_ft.resize(6); est_ee_ft.resize(6); des_ee_stiff.resize(6); shift_ee_ft.resize(6); for(int i=0; i<6; ++i) { des_ee_ft(i)=0; est_ee_ft(i)=0; shift_ee_ft(i)=0; } read_jpos.resize(numdof); joint_lims.resize(numdof); // TODO: need to find a better way to do this from launch files /********** Specific for RTKRobotArm (KUKA) ****************/ /* joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107; joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150;*/ /********** Specific for RTKRobotArm (BOXY) ****************/ joint_lims[0] = 150;joint_lims[1] = 107;joint_lims[2] = 150; joint_lims[3] = 107; joint_lims[4] = 150;joint_lims[5] = 115;joint_lims[6] = 150; joint_lims = joint_lims*M_PI/180.0; mRobot->setJointLimits(joint_lims); Eigen::VectorXd np; np.resize(numdof); np[0] = -0.62; np[1] = 0.76; np[2] = 0.61; np[3] = -1.0; np[4] = -0.40; np[5] = 1.4; np[6] = 1.4; // TODO: not implemented properly yet // mRobot->setNullPosture(np); /*********************************************************/ if(bUseIAI) { msg_vel_stiff_iai.stiffness.resize(numdof); msg_vel_stiff_iai.damping.resize(numdof); msg_vel_stiff_iai.add_torque.resize(numdof); msg_vel_stiff_iai.velocity.resize(numdof); } else { msg_vel_stiff_jstate.velocity.resize(numdof); } if(bUseIAI) { ROS_INFO_STREAM("Using iai_control_msgs"); pub_joints = nh.advertise<iai_control_msgs::MultiJointVelocityImpedanceCommand>(output_joints_topic, 3); } else { ROS_INFO_STREAM("Joints states"); // pub_joints = nh.advertise<sensor_msgs::JointState>(output_joints_topic, 3); pub_joints = nh.advertise<kuka_fri_bridge::JointStateImpedance>(output_joints_topic, 3); } ros::Subscriber sub_pos = nh.subscribe<geometry_msgs::PoseStamped>(input_pose_topic, 1, cartCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_ft_topic, 1, ftCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_stiff = nh.subscribe<geometry_msgs::WrenchStamped>(input_stiff_topic, 1, stiffnessCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_est_ft = nh.subscribe<geometry_msgs::WrenchStamped>(input_estimate_ft_topic, 1, estFTCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_jstate = nh.subscribe<sensor_msgs::JointState>(input_joint_topic, 1, jointStateCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub = nh.subscribe("Action_State", 1, actionStateCallback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub_ja = nh.subscribe<std_msgs::Bool>("joint_action", 1, jointActionCallback, ros::TransportHints().tcpNoDelay()); joint_vel.resize(numdof); joint_stiff.resize(numdof), joint_damp.resize(numdof); for(int i=0; i<numdof; ++i) { joint_vel[i] = 0.; joint_stiff[i] = DEFAULT_JSTIFF; joint_damp[i] = DEFAULT_JDAMP; } ROS_INFO("Waiting for robot joint state and FT estimate topic..."); ros::Rate r(1000); isJointOkay = false; isFTOkay = false; isAllOkay = false; while(ros::ok() && (!isJointOkay || (bUseForce && !isFTOkay)) ) { // while(ros::ok()){ ros::spinOnce(); r.sleep(); } tf::Pose tmp; mRobot->setJoints(read_jpos); mRobot->getEEPose(tmp); des_ee_pose = tmp; // Remove the residual force/torque from the future estimates. To compensate for error in ee force estimation or tool calibration shift_ee_ft = est_ee_ft; ROS_INFO_STREAM("Shift: "<<shift_ee_ft.transpose()); if(shift_ee_ft.norm() > ft_tol) { ROS_WARN("Shift in EE FT estimate more than the required tolerance!!"); ROS_WARN("Either increase the tolerance or calibrate the tool better"); ros::Duration warn(2); warn.sleep(); } ROS_INFO("Node started"); isAllOkay = true; // This is to ensure that we send atleast one zero velocity message before dying! // For safety. ros::Rate r_(1000); while(ros::ok()) { ros::spinOnce(); r_.sleep(); } for(int i=0; i<numdof; ++i) { joint_vel[i] = 0.0; } sendJointMessage(joint_vel, joint_stiff, joint_damp); ros::Duration(1.0).sleep(); nh.shutdown(); return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, "image_publisher"); ros::NodeHandle nh; ros::NodeHandle _nh("~"); // to get the private params image_transport::ImageTransport it(nh); image_transport::CameraPublisher pub = it.advertiseCamera("camera", 1); // provider can be an url (e.g.: rtsp://10.0.0.1:554) or a number of device, (e.g.: 0 would be /dev/video0) std::string video_stream_provider; cv::VideoCapture cap; if (_nh.getParam("video_stream_provider", video_stream_provider)){ ROS_INFO_STREAM("Resource video_stream_provider: " << video_stream_provider); // If we are given a string of 4 chars or less (I don't think we'll have more than 100 video devices connected) // treat is as a number and act accordingly so we open up the videoNUMBER device if (video_stream_provider.size() < 4){ ROS_INFO_STREAM("Getting video from provider: /dev/video" << video_stream_provider); cap.open(atoi(video_stream_provider.c_str())); } else{ ROS_INFO_STREAM("Getting video from provider: " << video_stream_provider); cap.open(video_stream_provider); } } else{ ROS_ERROR("Failed to get param 'video_stream_provider'"); return -1; } std::string camera_name; _nh.param("camera_name", camera_name, std::string("camera")); ROS_INFO_STREAM("Camera name: " << camera_name); int fps; _nh.param("fps", fps, 240); ROS_INFO_STREAM("Throttling to fps: " << fps); std::string frame_id; _nh.param("frame_id", frame_id, std::string("camera")); ROS_INFO_STREAM("Publishing with frame_id: " << frame_id); std::string camera_info_url; _nh.param("camera_info_url", camera_info_url, std::string("")); ROS_INFO_STREAM("Provided camera_info_url: '" << camera_info_url << "'"); bool flip_horizontal; _nh.param("flip_horizontal", flip_horizontal, false); ROS_INFO_STREAM("Flip horizontal image is : " << ((flip_horizontal)?"true":"false")); bool flip_vertical; _nh.param("flip_vertical", flip_vertical, false); ROS_INFO_STREAM("Flip flip_vertical image is : " << ((flip_vertical)?"true":"false")); // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#void flip(InputArray src, OutputArray dst, int flipCode) // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1 bool flip_image = true; int flip_value; if (flip_horizontal && flip_vertical) flip_value = 0; // flip both, horizontal and vertical else if (flip_horizontal) flip_value = 1; else if (flip_vertical) flip_value = -1; else flip_image = false; if(!cap.isOpened()){ ROS_ERROR_STREAM("Could not open the stream."); return -1; } ROS_INFO_STREAM("Opened the stream, starting to publish."); cv::Mat frame; sensor_msgs::ImagePtr msg; sensor_msgs::CameraInfo cam_info_msg; std_msgs::Header header; header.frame_id = frame_id; camera_info_manager::CameraInfoManager cam_info_manager(nh, camera_name, camera_info_url); // Get the saved camera info if any cam_info_msg = cam_info_manager.getCameraInfo(); ros::Rate r(fps); while (nh.ok()) { cap >> frame; if (pub.getNumSubscribers() > 0){ // Check if grabbed frame is actually full with some content if(!frame.empty()) { // Flip the image if necessary if (flip_image) cv::flip(frame, frame, flip_value); msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg(); // Create a default camera info if we didn't get a stored one on initialization if (cam_info_msg.distortion_model == ""){ ROS_WARN_STREAM("No calibration file given, publishing a reasonable default camera info."); cam_info_msg = get_default_camera_info_from_image(msg); cam_info_manager.setCameraInfo(cam_info_msg); } // The timestamps are in sync thanks to this publisher pub.publish(*msg, cam_info_msg, ros::Time::now()); } ros::spinOnce(); } r.sleep(); } }