int main(int argc, char **argv) { ros::init (argc, argv, "move_arm_pose_goal_test"); ros::NodeHandle nh; actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true); move_arm.waitForServer(); ROS_INFO("Connected to server"); arm_navigation_msgs::MoveArmGoal goalA; double x = 0.5, y = 0.1, z = 0.5, R = -M_PI_2, // greifer 'gerade' drehen P = M_PI_2, // M_PI_2 negativ: greifer zeigt nach oben. und umgekehrt Y = 0; // 0 0 0.6 -M_PI_2 M_PI_4 0 // 0.3 0 0.4 -M_PI_2 M_PI_2 0 // 0.5 0 0.3 -M_PI_2 M_PI_2 0 // 0.4 0.1 0.4 -M_PI_2 M_PI_2 -M_PI_2 if(argc >= 1+6) { // cmd name + args x = parseArg(argv[1]); y = parseArg(argv[2]); z = parseArg(argv[3]); R = parseArg(argv[4]); P = parseArg(argv[5]); Y = parseArg(argv[6]); } ROS_INFO_STREAM("x="<<x<<" y="<<y<<" z="<<z<<" R="<<R<<" P="<<P<<" Y="<<Y); goalA.motion_plan_request.group_name = "SchunkArm"; goalA.motion_plan_request.num_planning_attempts = 2; goalA.motion_plan_request.planner_id = std::string(""); goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0); arm_navigation_msgs::SimplePoseConstraint desired_pose; desired_pose.header.frame_id = "arm_base_link"; desired_pose.link_name = "gripper"; desired_pose.pose.position.x = x; desired_pose.pose.position.y = y; desired_pose.pose.position.z = z; double yaw = tan(desired_pose.pose.position.y / desired_pose.pose.position.x); ROS_INFO_STREAM("yaw="<<yaw); if(argc >= 1+7 && !strcmp(argv[7], "calcyaw")) { ROS_INFO("Using calculated yaw"); Y = yaw; } desired_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(R, P, Y); ROS_INFO_STREAM(desired_pose.pose.orientation); desired_pose.absolute_position_tolerance.x = 0.02; desired_pose.absolute_position_tolerance.y = 0.02; desired_pose.absolute_position_tolerance.z = 0.02; desired_pose.absolute_roll_tolerance = 0.04; desired_pose.absolute_pitch_tolerance = 0.04; desired_pose.absolute_yaw_tolerance = 0.04; arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA); if (nh.ok()) { bool finished_within_time = false; move_arm.sendGoal(goalA); finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); if (!finished_within_time) { move_arm.cancelGoal(); ROS_INFO("Timed out achieving goal A"); } else { actionlib::SimpleClientGoalState state = move_arm.getState(); bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); if(success) ROS_INFO("Action finished: %s",state.toString().c_str()); else ROS_INFO("Action failed: %s",state.toString().c_str()); } } ros::shutdown(); }
void callback(const sensor_msgs::ImageConstPtr &img, const sensor_msgs::CameraInfoConstPtr &info) { boost::mutex::scoped_lock lock(mutex_); ros::Time now = ros::Time::now(); static boost::circular_buffer<double> in_times(100); static boost::circular_buffer<double> out_times(100); static boost::circular_buffer<double> in_bytes(100); static boost::circular_buffer<double> out_bytes(100); ROS_DEBUG("resize: callback"); if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) { ROS_DEBUG("resize: number of subscribers is 0, ignoring image"); return; } in_times.push_front((now - last_subscribe_time_).toSec()); in_bytes.push_front(img->data.size()); // try { int width = dst_width_ ? dst_width_ : (resize_x_ * info->width); int height = dst_height_ ? dst_height_ : (resize_y_ * info->height); double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_; double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_; cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img); cv::Mat tmpmat(height, width, cv_img->image.type()); cv::resize(cv_img->image, tmpmat, cv::Size(width, height)); cv_img->image = tmpmat; sensor_msgs::CameraInfo tinfo = *info; tinfo.height = height; tinfo.width = width; tinfo.K[0] = tinfo.K[0] * scale_x; // fx tinfo.K[2] = tinfo.K[2] * scale_x; // cx tinfo.K[4] = tinfo.K[4] * scale_y; // fy tinfo.K[5] = tinfo.K[5] * scale_y; // cy tinfo.P[0] = tinfo.P[0] * scale_x; // fx tinfo.P[2] = tinfo.P[2] * scale_x; // cx tinfo.P[3] = tinfo.P[3] * scale_x; // T tinfo.P[5] = tinfo.P[5] * scale_y; // fy tinfo.P[6] = tinfo.P[6] * scale_y; // cy if ( !use_messages_ || now - last_publish_time_ > period_ ) { cp_.publish(cv_img->toImageMsg(), boost::make_shared<sensor_msgs::CameraInfo> (tinfo)); out_times.push_front((now - last_publish_time_).toSec()); out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize()); last_publish_time_ = now; } } catch( cv::Exception& e ) { ROS_ERROR("%s", e.what()); } float duration = (now - last_rosinfo_time_).toSec(); if ( duration > 2 ) { int in_time_n = in_times.size(); int out_time_n = out_times.size(); double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta; double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta; std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) ); in_time_mean /= in_time_n; in_time_rate /= in_time_mean; std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) ); in_time_std_dev = sqrt(in_time_std_dev/in_time_n); if ( in_time_n > 1 ) { in_time_min_delta = *std::min_element(in_times.begin(), in_times.end()); in_time_max_delta = *std::max_element(in_times.begin(), in_times.end()); } std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) ); out_time_mean /= out_time_n; out_time_rate /= out_time_mean; std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) ); out_time_std_dev = sqrt(out_time_std_dev/out_time_n); if ( out_time_n > 1 ) { out_time_min_delta = *std::min_element(out_times.begin(), out_times.end()); out_time_max_delta = *std::max_element(out_times.begin(), out_times.end()); } double in_byte_mean = 0, out_byte_mean = 0; std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) ); std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) ); in_byte_mean /= duration; out_byte_mean /= duration; ROS_INFO_STREAM(" in bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << in_byte_mean/1000*8 << " Kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate << " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta << " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n); ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << out_byte_mean/1000*8 << " kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate << " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta << " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n); in_times.clear(); in_bytes.clear(); out_times.clear(); out_bytes.clear(); last_rosinfo_time_ = now; } last_subscribe_time_ = now; if(use_snapshot_) { publish_once_ = false; } }
int main(int argc, char **argv) { if (argc < 2) { std::cout << "This program need one input parameter.\n"<< "The first input parameter is the number of the UAV." << std::endl; return -1; } // The UAV ID is stored in a global variable std::string uavID="aal_"; uavID.append(std::string(argv[1])); signal(SIGINT, quit); string node_name="arm_joystick"; node_name.append(std::string(argv[1])); ros::init(argc, argv, node_name.c_str()); ros::NodeHandle n(uavID); string topicname = "/cmd_vel"; //Publisher and Subscribers ros::Publisher arm_control_ref_pub = n.advertise<arcas_msgs::ArmControlReferencesStamped>("/aal_1/arm_control_references",0); ros::Subscriber state_estimation_subscriber = n.subscribe("/aal_1/arm_state_estimation", 1000, &armStateEstimationCallback); int fd; struct wwvi_js_event wjse; wjse.stick2_x = 0; // Roll wjse.stick2_y = 0; // Yaw wjse.stick_x = 0; // Pitch wjse.stick_y = 0; // Thrust wjse.button[0]=0; // Take-Off, Button 1 on Joystick wjse.button[1]=0; // Land, Button 2 on Joystick CJoystick joystick; fd = joystick.open_joystick("/dev/input/js1"); // Initialize ActionClient AALExtensionActionWrapper aalExtensionActionWrapper; if(!fd) { cerr << "Can not open joystick, is it still connected?\r\n" << endl; return(-1); } ros::AsyncSpinner spinner_(0); spinner_.start(); while(ros::ok()) { // Calculate the quad_control_references if(wjse.stick_x!=0) { arm_control_references.arm_control_references.position_ref[2] = arm_state_estimation.arm_state_estimation.position[2] + wjse.stick_x/6000.0; } if(wjse.stick2_x!=0) { arm_control_references.arm_control_references.position_ref[3] = arm_state_estimation.arm_state_estimation.position[3] + wjse.stick2_x/6000.0; } if(wjse.stick_y!=0) { arm_control_references.arm_control_references.position_ref[1] = arm_state_estimation.arm_state_estimation.position[1] + wjse.stick_y/6000.0; } if( wjse.stick2_y!=0) { arm_control_references.arm_control_references.position_ref[0] = arm_state_estimation.arm_state_estimation.position[0] + wjse.stick2_y/6000.0; } if(wjse.stick3_x!=0) { arm_control_references.arm_control_references.position_ref[4] = arm_state_estimation.arm_state_estimation.position[4] + wjse.stick3_x/12000.0; } if(wjse.stick3_y!=0) { arm_control_references.arm_control_references.position_ref[5] = arm_state_estimation.arm_state_estimation.position[5] + wjse.stick3_y/12000.0; } /* if(joystick.get_joystick_status(&wjse)!=-1) { command_to_send.linear.x = wjse.stick_x/2500.0; //std::cout << "Pitch control: " << command_to_send.linear.x << std::endl; command_to_send.linear.y = wjse.stick2_x/2500.0; //std::cout << "Roll control: " << command_to_send.linear.y << std::endl; command_to_send.linear.z = wjse.stick_y/4000.0; //std::cout << "Thrust control: " << command_to_send.linear.z << std::endl; command_to_send.angular.z = wjse.stick2_y/4000.0; //std::cout << "Yaw control: " << command_to_send.angular.z << std::endl; } else { ROS_ERROR_STREAM("Problems with joystick!"); } */ // Implement take-off if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[0] == 1)) { ROS_INFO_STREAM("Extracting Sent"); aalExtensionActionWrapper.extendArm(); usleep(100000); // In order to not send it twice accidentally } // Implement landing if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[1] == 1)) { ROS_INFO_STREAM("Contracting Sent"); aalExtensionActionWrapper.contractArm(); usleep(100000); // In order to not send it twice accidentally } if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[2] == 1)) { arm_control_references.arm_control_references.position_ref[6] = 3.14159/5; usleep(100000); // In order to not send it twice accidentally } if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[3] == 1)) { arm_control_references.arm_control_references.position_ref[6] =0.0; usleep(100000); // In order to not send it twice accidentally } //joystick_publisher.publish(command_to_send); arm_control_ref_pub.publish(arm_control_references); //~20Hz usleep(50000); } return 0; }
/* * Get the list of controller names. */ virtual void getControllersList(std::vector<std::string> &names) { for (std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it) names.push_back(it->first); ROS_INFO_STREAM("Returned " << names.size() << " controllers in list"); }
int main( int argc, char **argv ) { ros::init( argc, argv, "example2" ); ros::NodeHandle n; const double val = 3.14; // Basic messages: ROS_INFO( "My INFO message." ); ROS_INFO( "My INFO message with argument: %f", val ); ROS_INFO_STREAM( "My INFO stream message with argument: " << val ); // Named messages: ROS_INFO_STREAM_NAMED( "named_msg", "My named INFO stream message; val = " << val ); // Conditional messages: ROS_INFO_STREAM_COND( val < 0., "My conditional INFO stream message; val (" << val << ") < 0" ); ROS_INFO_STREAM_COND( val >= 0., "My conditional INFO stream message; val (" << val << ") >= 0" ); // Conditional Named messages: ROS_INFO_STREAM_COND_NAMED( val < 0., "cond_named_msg", "My conditional INFO stream message; val (" << val << ") < 0" ); ROS_INFO_STREAM_COND_NAMED( val >= 0., "cond_named_msg", "My conditional INFO stream message; val (" << val << ") >= 0" ); // Filtered messages: struct MyLowerFilter : public ros::console::FilterBase { MyLowerFilter( const double& val ) : value( val ) {} inline virtual bool isEnabled() { return value < 0.; } double value; }; struct MyGreaterEqualFilter : public ros::console::FilterBase { MyGreaterEqualFilter( const double& val ) : value( val ) {} inline virtual bool isEnabled() { return value >= 0.; } double value; }; MyLowerFilter filter_lower( val ); MyGreaterEqualFilter filter_greater_equal( val ); ROS_INFO_STREAM_FILTER( &filter_lower, "My filter INFO stream message; val (" << val << ") < 0" ); ROS_INFO_STREAM_FILTER( &filter_greater_equal, "My filter INFO stream message; val (" << val << ") >= 0" ); // Once messages: for( int i = 0; i < 10; ++i ) { ROS_INFO_STREAM_ONCE( "My once INFO stream message; i = " << i ); } // Throttle messages: for( int i = 0; i < 10; ++i ) { ROS_INFO_STREAM_THROTTLE( 2, "My throttle INFO stream message; i = " << i ); ros::Duration( 1 ).sleep(); } ros::spinOnce(); return EXIT_SUCCESS; }
int main(int argc, char ** argv) { ros::init(argc, argv, "mpu_calculate"); ros::NodeHandle nh; //ros::Subscriber imu_data = nh.subscribe(); ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("imudata",1); //串口读取数据 // serial::Serial serport; // try // { // //打开串口0 // serport.setPort("/dev/ttyUSB0"); // serport.setBaudrate(115200); // serial::Timeout to = serial::Timeout::simpleTimeout(0); // serport.setTimeout(to); // serport.open(); // } // catch(serial::IOException & e) // { // ROS_ERROR_STREAM("Unable to open serial port"); // return -1; // } // //判断串口是否打开 // if(serport.isOpen()) // { // ROS_INFO_STREAM("Serial Port initialized"); // } // else // { // return -1; // } // serport.flushInput(); //uint8 dataPacket[PKGSIZE]; m_serial.init(); sensor_msgs::Imu imu_msg; //判断是否已经标定过 bool calibrated = false; double zeroshift[3]; int times = 0; ROS_INFO_STREAM("Reading from serial port"); std::cout << "start calibrate gyro "; ros::Rate loop_rate(200); double starttime, endtime; while(ros::ok()) { starttime = ros::Time::now().toSec(); ros::spinOnce(); static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, 0); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "imu")); // int datasize = serport.read(dataPacket, packetLen); m_serial.Read(); if (m_serial.isReadOK == 1) { m_serial.isReadOK = 0; if(parseimumsg(m_serial.readbuff, &imu_msg) == true) { //解析imu的datapacket if(!calibrated) { ROS_INFO_STREAM("Calibrating"); if(times == 200) { std::cout << std::endl; calibrated = true; zeroshift[0] /= 200; zeroshift[1] /= 200; zeroshift[2] /= 200; Global_Initialise(roll_init/200.0,pitch_init/200.0,yaw_init/200.0); } else { zeroshift[0] += imu_msg.angular_velocity.x; zeroshift[1] += imu_msg.angular_velocity.y; zeroshift[2] += imu_msg.angular_velocity.z; GetInitAng(&imu_msg); std::cout << "."; } times += 1; } else { imu_msg.angular_velocity.x -= zeroshift[0]; imu_msg.angular_velocity.y -= zeroshift[1]; imu_msg.angular_velocity.z -= zeroshift[2]; // std::cout << "--------------------------------" << std::endl; // std::cout << imu_msg.angular_velocity.x <<" "<< imu_msg.angular_velocity.y << " " << imu_msg.angular_velocity.z << std::endl; // std::cout << imu_msg.linear_acceleration.x <<" "<< imu_msg.linear_acceleration.y <<" "<< imu_msg.linear_acceleration.z << std::endl; imuPub.publish(imu_msg); pixhawk_ekf(imu_msg.angular_velocity.x,imu_msg.angular_velocity.y,imu_msg.angular_velocity.z,imu_msg.linear_acceleration.x,imu_msg.linear_acceleration.y,imu_msg.linear_acceleration.z); //ROS_INFO_STREAM("OK!"); } } } //发送数据 //endtime = ros::Time::now().toSec(); //std::cout << "time" << endtime - starttime << std::endl; loop_rate.sleep(); } }
void GripperAction::goalCB(GoalHandle gh) { // Cancels the currently active goal. if (has_active_goal_) { // Marks the current goal as canceled. active_goal_.setCanceled(); has_active_goal_ = false; std::cout << "canceled current active goal" << std::endl; } gh.setAccepted(); active_goal_ = gh; has_active_goal_ = true; goal_received_ = ros::Time::now(); min_error_seen_ = 1e10; ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort); ROS_INFO("setFingersValues"); std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle(); std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl; target_position = active_goal_.getGoal()->command.position; target_effort = active_goal_.getGoal()->command.max_effort; //determine if the gripper is supposed to be opened or closed double current_position = jaco_->getFingersJointAngle()[0]; if(current_position > target_position){ opening = true; std::cout << "Opening gripper" << std::endl; }else{ opening = false; std::cout << "Closing gripper" << std::endl; } fingerPositionsRadian[0] = target_position; fingerPositionsRadian[1] = target_position; fingerPositionsRadian[2] = target_position; std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl; double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])}; if(!jaco_->setFingersValues(fingerPositionsDegree)) { active_goal_.setCanceled(); ROS_ERROR("Cancelling goal: moveJoint didn't work."); std::cout << "Cancelling goal because setFingers didn't work" << std::endl; } last_movement_time_ = ros::Time::now(); }
int getCamCalibration(string dir_root, string camera_name, double* K,std::vector<double> & D,double *R,double* P){ /* * from: http://kitti.is.tue.mpg.de/kitti/devkit_raw_data.zip * calib_cam_to_cam.txt: Camera-to-camera calibration * -------------------------------------------------- * * - S_xx: 1x2 size of image xx before rectification * - K_xx: 3x3 calibration matrix of camera xx before rectification * - D_xx: 1x5 distortion vector of camera xx before rectification * - R_xx: 3x3 rotation matrix of camera xx (extrinsic) * - T_xx: 3x1 translation vector of camera xx (extrinsic) * - S_rect_xx: 1x2 size of image xx after rectification * - R_rect_xx: 3x3 rectifying rotation to make image planes co-planar * - P_rect_xx: 3x4 projection matrix after rectification */ // double K[9]; // Calibration Matrix // double D[5]; // Distortion Coefficients // double R[9]; // Rectification Matrix // double P[12]; // Projection Matrix Rectified (u,v,w) = P * R * (x,y,z,q) string calib_cam_to_cam=(fs::path(dir_root) / fs::path("calib_cam_to_cam.txt")).string(); ifstream file_c2c(calib_cam_to_cam.c_str()); if (!file_c2c.is_open()) return false; ROS_INFO_STREAM("Reading camera" << camera_name << " calibration from " << calib_cam_to_cam); typedef boost::tokenizer<boost::char_separator<char> > tokenizer; boost::char_separator<char> sep{" "}; string line=""; char index=0; tokenizer::iterator token_iterator; while (getline(file_c2c,line)) { // Parse string phase 1, tokenize it using Boost. tokenizer tok(line,sep); // Move the iterator at the beginning of the tokenize vector and check for K/D/R/P matrices. token_iterator=tok.begin(); if (strcmp((*token_iterator).c_str(),((string)(string("K_")+camera_name+string(":"))).c_str())==0) //Calibration Matrix { index=0; //should be 9 at the end ROS_DEBUG_STREAM("K_" << camera_name); for (token_iterator++; token_iterator != tok.end(); token_iterator++) { //std::cout << *token_iterator << '\n'; K[index++]=boost::lexical_cast<double>(*token_iterator); } } // EXPERIMENTAL: use with unrectified images // token_iterator=tok.begin(); // if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients // { // index=0; //should be 5 at the end // ROS_DEBUG_STREAM("D_" << camera_name); // for (token_iterator++; token_iterator != tok.end(); token_iterator++) // { //// std::cout << *token_iterator << '\n'; // D[index++]=boost::lexical_cast<double>(*token_iterator); // } // } token_iterator=tok.begin(); if (strcmp((*token_iterator).c_str(),((string)(string("R_")+camera_name+string(":"))).c_str())==0) //Rectification Matrix { index=0; //should be 12 at the end ROS_DEBUG_STREAM("R_" << camera_name); for (token_iterator++; token_iterator != tok.end(); token_iterator++) { //std::cout << *token_iterator << '\n'; R[index++]=boost::lexical_cast<double>(*token_iterator); } } token_iterator=tok.begin(); if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified { index=0; //should be 12 at the end ROS_DEBUG_STREAM("P_rect_" << camera_name); for (token_iterator++; token_iterator != tok.end(); token_iterator++) { //std::cout << *token_iterator << '\n'; P[index++]=boost::lexical_cast<double>(*token_iterator); } } } ROS_INFO_STREAM("... ok"); return true; }
void GSPipe::publish_stream() { ROS_INFO_STREAM("Publishing stream..."); // Pre-roll camera if needed if (preroll_) { ROS_DEBUG("Performing preroll..."); //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll //I am told this is needed and am erring on the side of caution. gst_element_set_state(pipeline_, GST_STATE_PLAYING); if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Failed to PLAY during preroll."); return; } else { ROS_DEBUG("Stream is PLAYING in preroll."); } gst_element_set_state(pipeline_, GST_STATE_PAUSED); if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Failed to PAUSE."); return; } else { ROS_INFO("Stream is PAUSED in preroll."); } } if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Could not start stream!"); return; } ROS_INFO("Started stream."); // Poll the data as fast a spossible while(ros::ok()) { // This should block until a new frame is awake, this way, we'll run at the // actual capture framerate of the device. ROS_DEBUG("Getting data..."); GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_)); GstFormat fmt = GST_FORMAT_TIME; gint64 current = -1; // Query the current position of the stream //if (gst_element_query_position(pipeline_, &fmt, ¤t)) { //ROS_INFO_STREAM("Position "<<current); //} // Stop on end of stream if (!buf) { ROS_INFO("Stream ended."); break; } ROS_DEBUG("Got data."); // Get the image width and height GstPad* pad = gst_element_get_static_pad(sink_, "sink"); const GstCaps *caps = gst_pad_get_negotiated_caps(pad); GstStructure *structure = gst_caps_get_structure(caps,0); gst_structure_get_int(structure,"width",&width_); gst_structure_get_int(structure,"height",&height_); // Complain if the returned buffer is smaller than we expect const unsigned int expected_frame_size = image_encoding_ == sensor_msgs::image_encodings::RGB8 ? width_ * height_ * 3 : width_ * height_; if (buf->size < expected_frame_size) { ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be " << expected_frame_size << " bytes but got only " << (buf->size) << " bytes. (make sure frames are correctly encoded)"); } // Construct Image message sensor_msgs::ImagePtr img(new sensor_msgs::Image()); sensor_msgs::CameraInfoPtr cinfo; // Update header information cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo())); cinfo->header.stamp = ros::Time::now(); cinfo->header.frame_id = frame_id_; img->header = cinfo->header; // Image data and metadata img->width = width_; img->height = height_; img->encoding = image_encoding_; img->is_bigendian = false; img->data.resize(expected_frame_size); ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_); // Copy only the data we received // Since we're publishing shared pointers, we need to copy the image so // we can free the buffer allocated by gstreamer if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { img->step = width_ * 3; } else { img->step = width_; } std::copy( buf->data, (buf->data)+(buf->size), img->data.begin()); // Publish the image/info camera_pub_.publish(img, cinfo); // Release the buffer gst_buffer_unref(buf); ros::spinOnce(); } }
// Initialization process void EkfNode::init() { /************************************************************************** * Initialize firstRun, time, and frame names **************************************************************************/ // Set first run to true for encoders. Once a message is received, this will // be set to false. firstRunEnc_ = true; firstRunSys_ = true; // Set Times ros::Time currTime = ros::Time::now(); lastEncTime_ = currTime; lastSysTime_ = currTime; // Use the ROS parameter server to initilize parameters if(!private_nh_.getParam("base_frame", base_frame_)) base_frame_ = "base_link"; if(!private_nh_.getParam("odom_frame", base_frame_)) odom_frame_ = "odom"; if(!private_nh_.getParam("map_frame", map_frame_)) map_frame_ = "map"; /************************************************************************** * Initialize state for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize state double state_temp[] = {0, 0, 0, 0, 0, 0}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double)); // Check the parameter server and initialize state if(!private_nh_.getParam("state", state)) { ROS_WARN_STREAM("No state found. Using default."); } // Check to see if the size is not equal to 6 if (state.size() != 6) { ROS_WARN_STREAM("state isn't 6 elements long!"); } // And initialize the Matrix typedef Matrix<double, 6, 1> Vector6d; Vector6d stateMat(state.data()); std::cout << "state_map = " << std::endl; std::cout << stateMat << std::endl; ekf_map_.initState(stateMat); // Odom is always initialized at all zeros. stateMat << 0, 0, 0, 0, 0, 0; ekf_odom_.initState(stateMat); std::cout << "state_odom = " << std::endl; std::cout << stateMat << std::endl; /************************************************************************** * Initialize covariance for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize covariance double cov_temp[] = {0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double)); // Check the parameter server and initialize cov if(!private_nh_.getParam("covariance", cov)) { ROS_WARN_STREAM("No covariance found. Using default."); } // Check to see if the size is not equal to 36 if (cov.size() != 36) { ROS_WARN_STREAM("cov isn't 36 elements long!"); } // And initialize the Matrix typedef Matrix<double, 6, 6, RowMajor> Matrix66; Matrix66 covMat(cov.data()); std::cout << "covariance = " << std::endl; std::cout << covMat << std::endl; ekf_map_.initCov(covMat); // Initialize odom covariance the same as the map covariance (this isn't // correct. But, since it is all an estimate anyway, it should be fine. ekf_odom_.initCov(covMat); /************************************************************************** * Initialize Q for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize Q double Q_temp[] = {0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double)); // Check the parameter server and initialize Q if(!private_nh_.getParam("Q", Q)) { ROS_WARN_STREAM("No Q found. Using default."); } // Check to see if the size is not equal to 36 if (Q.size() != 36) { ROS_WARN_STREAM("Q isn't 36 elements long!"); } // And initialize the Matrix Matrix66 QMat(Q.data()); std::cout << "Q = " << std::endl; std::cout << QMat << std::endl; ekf_map_.initSystem(QMat); ekf_odom_.initSystem(QMat); /************************************************************************** * Initialize Decawave for ekf_map_ (not used in ekf_odom_) **************************************************************************/ // Create temp array to initialize R for DecaWave double RDW_temp[] = {0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double)); // Check the parameter server and initialize RDW if(!private_nh_.getParam("DW_R", RDW)) { ROS_WARN_STREAM("No DW_R found. Using default."); } // Check to see if the size is not equal to 16 if (RDW.size() != 16) { ROS_WARN_STREAM("DW_R isn't 16 elements long!"); } // And initialize the Matrix typedef Matrix<double, 4, 4, RowMajor> Matrix44; Matrix44 RDWMat(RDW.data()); std::cout << "RDecaWave = " << std::endl; std::cout << RDWMat << std::endl; // Create temp array to initialize beacon locations for DecaWave double DWBL_temp[] = {0, 0, 5, 0, 5, 15, 0, 15}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double)); // Check the parameter server and initialize DWBL if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) { ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default."); } // Check to see if the size is not equal to 8 if (DWBL.size() != 8) { ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!"); } // And initialize the Matrix typedef Matrix<double, 4, 2, RowMajor> Matrix42; Matrix42 DWBLMat(DWBL.data()); std::cout << "DW_Beacon_Loc = " << std::endl; std::cout << DWBLMat << std::endl; MatrixXd DecaWaveBeaconLoc(4,2); MatrixXd DecaWaveOffset(2,1); double DW_offset_x; double DW_offset_y; if(!private_nh_.getParam("DW_offset_x", DW_offset_x)) DW_offset_x = 0.0; if(!private_nh_.getParam("DW_offset_y", DW_offset_y)) DW_offset_y = 0.0; DecaWaveOffset << DW_offset_x, DW_offset_y; std::cout << "DecaWaveOffset = " << std::endl; std::cout << DecaWaveOffset << std::endl; ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset); // Decawave is not used in odom, so no need to initialize /************************************************************************** * Initialize Encoders for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize R for DecaWave double REnc_temp[] = {0.01, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double)); // Check the parameter server and initialize RDW if(!private_nh_.getParam("Enc_R", REnc)) { ROS_WARN_STREAM("No Enc_R found. Using default."); } // Check to see if the size is not equal to 4 if (REnc.size() != 4) { ROS_WARN_STREAM("Enc_R isn't 4 elements long!"); } // And initialize the Matrix typedef Matrix<double, 2, 2, RowMajor> Matrix22; Matrix22 REncMat(REnc.data()); std::cout << "R_Enc = " << std::endl; std::cout << REncMat << std::endl; double b, tpmRight, tpmLeft; if(!private_nh_.getParam("track_width", b)) b = 1; if(!private_nh_.getParam("tpm_right", tpmRight)) tpmRight = 1; if(!private_nh_.getParam("tpm_left", tpmLeft)) tpmLeft = 1; ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft); ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft); std::cout << "track_width = " << b << std::endl; std::cout << "tpm_right = " << tpmRight << std::endl; std::cout << "tpm_left = " << tpmLeft << std::endl; /************************************************************************** * Initialize IMU for ekf_odom_ and ekf_map_ **************************************************************************/ double RIMU; if(!private_nh_.getParam("R_IMU", RIMU)) RIMU = 0.01; ekf_map_.initIMU(RIMU); ekf_odom_.initIMU(RIMU); std::cout << "R_IMU = " << RIMU << std::endl; // Publish the initialized state and exit initialization. publishState(currTime); ROS_INFO_STREAM("Finished with initialization."); }
int main(int argc, char** argv) { ros::init(argc, argv, "example_cart_move_action_client"); // name this node ros::NodeHandle nh; //standard ros node handle ArmMotionCommander arm_motion_commander(&nh); CwruPclUtils cwru_pcl_utils(&nh); while (!cwru_pcl_utils.got_kinect_cloud()) { ROS_INFO("did not receive pointcloud"); ros::spinOnce(); ros::Duration(1.0).sleep(); } ROS_INFO("got a pointcloud"); tf::StampedTransform tf_sensor_frame_to_torso_frame; //use this to transform sensor frame to torso frame tf::TransformListener tf_listener; //start a transform listener //let's warm up the tf_listener, to make sure it get's all the transforms it needs to avoid crashing: bool tferr = true; ROS_INFO("waiting for tf between kinect_pc_frame and torso..."); while (tferr) { tferr = false; try { //The direction of the transform returned will be from the target_frame to the source_frame. //Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame); } catch (tf::TransformException &exception) { ROS_ERROR("%s", exception.what()); tferr = true; ros::Duration(0.5).sleep(); // sleep for half a second ros::spinOnce(); } } ROS_INFO("tf is good"); //tf-listener found a complete chain from sensor to world; ready to roll //convert the tf to an Eigen::Affine: Eigen::Affine3f A_sensor_wrt_torso; Eigen::Affine3d Affine_des_gripper; Eigen::Vector3d xvec_des,yvec_des,zvec_des,origin_des; geometry_msgs::PoseStamped rt_tool_pose; A_sensor_wrt_torso = cwru_pcl_utils.transformTFToEigen(tf_sensor_frame_to_torso_frame); Eigen::Vector3f plane_normal, major_axis, centroid; Eigen::Matrix3d Rmat; int rtn_val; double plane_dist; //send a command to plan a joint-space move to pre-defined pose: rtn_val=arm_motion_commander.plan_move_to_pre_pose(); //send command to execute planned motion rtn_val=arm_motion_commander.rt_arm_execute_planned_path(); while (ros::ok()) { if (cwru_pcl_utils.got_selected_points()) { ROS_INFO("transforming selected points"); cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso); //fit a plane to these selected points: cwru_pcl_utils.fit_xformed_selected_pts_to_plane(plane_normal, plane_dist); ROS_INFO_STREAM(" normal: " << plane_normal.transpose() << "; dist = " << plane_dist); major_axis= cwru_pcl_utils.get_major_axis(); centroid = cwru_pcl_utils.get_centroid(); cwru_pcl_utils.reset_got_selected_points(); // reset the selected-points trigger //construct a goal affine pose: for (int i=0;i<3;i++) { origin_des[i] = centroid[i]; // convert to double precision zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal xvec_des[i] = major_axis[i]; } origin_des[2]+=0.02; //raise up 2cm yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad Rmat.col(0)= xvec_des; Rmat.col(1)= yvec_des; Rmat.col(2)= zvec_des; Affine_des_gripper.linear()=Rmat; Affine_des_gripper.translation()=origin_des; cout<<"des origin: "<<Affine_des_gripper.translation().transpose()<<endl; cout<<"orientation: "<<endl; cout<<Rmat<<endl; //convert des pose from Eigen::Affine to geometry_msgs::PoseStamped rt_tool_pose.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper); //could fill out the header as well... // send move plan request: rtn_val=arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose); if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { //send command to execute planned motion rtn_val=arm_motion_commander.rt_arm_execute_planned_path(); } else { ROS_WARN("Cartesian path to desired pose not achievable"); } } ros::Duration(0.5).sleep(); // sleep for half a second ros::spinOnce(); } return 0; }
// Publish the state as an odom message on the topic odom_ekf. Alos well broadcast a transform. void EkfNode::publishState(ros::Time now){ // Create an Odometry message to publish nav_msgs::Odometry state_msg; /************************************************************************** * Populate Odometry message for ekf_map_ **************************************************************************/ // Populate timestamp, position frame, and velocity frame state_msg.header.stamp = now; state_msg.header.frame_id = map_frame_; state_msg.child_frame_id = base_frame_; // Populate the position and orientation state_msg.pose.pose.position.x = ekf_map_.state_(0); // x state_msg.pose.pose.position.y = ekf_map_.state_(1); // y state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_map_.state_(2)); // theta boost::array<double,36> temp; // Populate the covariance matrix state_msg.pose.covariance = temp; // Populate the linear and angular velocities state_msg.twist.twist.linear.x = ekf_map_.state_(3); // v state_msg.twist.twist.angular.z = ekf_map_.state_(4); // omega // Populate the covariance matrix state_msg.twist.covariance = temp; // Publish the message! stateMapPub_.publish(state_msg); // Print for debugging purposes ROS_INFO_STREAM("\nstate = \n" << ekf_map_.state_); ROS_INFO_STREAM("\ncovariance = \n" << ekf_map_.cov_); /************************************************************************** * Populate Odometry message for ekf_odom_ **************************************************************************/ // Populate timestamp, position frame, and velocity frame state_msg.header.stamp = now; state_msg.header.frame_id = odom_frame_; state_msg.child_frame_id = base_frame_; // Populate the position and orientation state_msg.pose.pose.position.x = ekf_odom_.state_(0); // x state_msg.pose.pose.position.y = ekf_odom_.state_(1); // y state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_odom_.state_(2)); // theta // Populate the covariance matrix state_msg.pose.covariance = temp; // Populate the linear and angular velocities state_msg.twist.twist.linear.x = ekf_odom_.state_(3); // v state_msg.twist.twist.angular.z = ekf_odom_.state_(4); // omega // Populate the covariance matrix state_msg.twist.covariance = temp; // Publish the message! stateOdomPub_.publish(state_msg); /************************************************************************** * Create and broadcast transforms for map->odom and odom->base_link **************************************************************************/ static tf::TransformBroadcaster br; // First create and broadcast odom->base_link since we already have that. tf::Transform odom2base; odom2base.setOrigin(tf::Vector3(ekf_odom_.state_(0), ekf_odom_.state_(1), 0.0)); tf::Quaternion q; q.setRPY(0.0, 0.0, ekf_odom_.state_(2)); odom2base.setRotation(q); br.sendTransform(tf::StampedTransform(odom2base, now, odom_frame_, base_frame_)); // Now create and broadcast map->odom. But, first we must calculate this from // map->base_link and odom->base_link. tf::Transform map2odom; double x2 = ekf_odom_.state_(0); double y2 = ekf_odom_.state_(1); double theta2 = ekf_odom_.state_(2); double x3 = ekf_map_.state_(0); double y3 = ekf_map_.state_(1); double theta3 = ekf_map_.state_(2); double x1 = cos(theta3)*(-x2*cos(theta2)-y2*sin(theta2)) -sin(theta3)*( x2*sin(theta2)-y2*cos(theta2)) +x3; double y1 = sin(theta3)*(-x2*cos(theta2)-y2*sin(theta2)) +cos(theta3)*( x2*sin(theta2)-y2*cos(theta2)) +y3; double theta1 = theta3 - theta2; map2odom.setOrigin(tf::Vector3(x1, y1, 0.0)); q.setRPY(0.0, 0.0, theta1); map2odom.setRotation(q); br.sendTransform(tf::StampedTransform(map2odom, now, map_frame_, odom_frame_)); }
// Processes the point cloud with OpenCV using the PCL cluster indices void processClusters( const std::vector<pcl::PointIndices> cluster_indices, // const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg, const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed, const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered, const pcl::PointCloud<pcl::PointXYZRGB>& cloud ) { // ------------------------------------------------------------------------------------------------------- // Convert image ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format"); try { sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image); // pcl::toROSMsg (*pointcloud_msg, *image_msg); pcl::toROSMsg (*cloud_transformed, *image_msg); cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8"); full_input_image = input_bridge->image; } catch (cv_bridge::Exception& ex) { ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image"); return; } // ------------------------------------------------------------------------------------------------------- // Process Image // Convert image to gray cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY ); //cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10); // Blur image - reduce noise with a 3x3 kernel cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) ); ROS_INFO_STREAM_NAMED("perception","Finished coverting"); // ------------------------------------------------------------------------------------------------------- // Check OpenCV and PCL image height for errors int image_width = cloud.width; int image_height = cloud.height; ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n"); int image_width_cv = full_input_image.size.p[1]; int image_height_cv = full_input_image.size.p[0]; ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n"); if( image_width != image_width_cv || image_height != image_height_cv ) { ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!"); return; } // ------------------------------------------------------------------------------------------------------- // GUI Stuff // First window const char* opencv_window = "Source"; /* cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE ); cv::imshow( opencv_window, full_input_image_gray ); */ // while(true) // use this when we want to tweak the image { output_image = full_input_image.clone(); // ------------------------------------------------------------------------------------------------------- // Start processing clusters ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis"); int top_image_overlay_x = 0; // tracks were to copyTo the mini images // for each cluster, see if it is a block for(size_t c = 0; c < cluster_indices.size(); ++c) { ROS_INFO_STREAM_NAMED("perception","\n\n"); ROS_INFO_STREAM_NAMED("perception","On cluster " << c); // find the outer dimensions of the cluster double xmin = 0; double xmax = 0; double ymin = 0; double ymax = 0; // also remember each min & max's correponding other coordinate (not needed for z) double xminy = 0; double xmaxy = 0; double yminx = 0; double ymaxx = 0; // also remember their corresponding indice int xmini = 0; int xmaxi = 0; int ymini = 0; int ymaxi = 0; // loop through and find all min/max of x/y for(size_t i = 0; i < cluster_indices[c].indices.size(); i++) { int j = cluster_indices[c].indices[i]; // Get RGB from point cloud pcl::PointXYZRGB p = cloud_transformed->points[j]; double x = p.x; double y = p.y; if(i == 0) // initial values { xmin = xmax = x; ymin = ymax = y; xminy = xmaxy = y; yminx = ymaxx = x; xmini = xmaxi = ymini = ymaxi = j; // record the indice corresponding to the min/max } else { if( x < xmin ) { xmin = x; xminy = y; xmini = j; } if( x > xmax ) { xmax = x; xmaxy = y; xmaxi = j; } if( y < ymin ) { ymin = y; yminx = x; ymini = j; } if( y > ymax ) { ymax = y; ymaxx = x; ymaxi = j; } } } ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmin: " << xmin << " xmax: " << xmax << " ymin: " << ymin << " ymax: " << ymax); ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmini: " << xmini << " xmaxi: " << xmaxi << " ymini: " << ymini << " ymaxi: " << ymaxi); // --------------------------------------------------------------------------------------------- // Check if these dimensions make sense for the block size specified double xside = xmax-xmin; double yside = ymax-ymin; const double tol = 0.01; // 1 cm error tolerance // In order to be part of the block, xside and yside must be between // blocksize and blocksize*sqrt(2) if(xside > block_size-tol && xside < block_size*sqrt(2)+tol && yside > block_size-tol && yside < block_size*sqrt(2)+tol ) { // ------------------------------------------------------------------------------------------------------- // Get the four farthest corners of the block - use OpenCV only on the region identified by PCL // Get the pixel coordinates of the xmax and ymax indicies int px_xmax = 0; int py_xmax = 0; int px_ymax = 0; int py_ymax = 0; getXYCoordinates( xmaxi, image_height, image_width, px_xmax, py_xmax); getXYCoordinates( ymaxi, image_height, image_width, px_ymax, py_ymax); // Get the pixel coordinates of the xmin and ymin indicies int px_xmin = 0; int py_xmin = 0; int px_ymin = 0; int py_ymin = 0; getXYCoordinates( xmini, image_height, image_width, px_xmin, py_xmin); getXYCoordinates( ymini, image_height, image_width, px_ymin, py_ymin); ROS_DEBUG_STREAM_NAMED("perception","px_xmin " << px_xmin << " px_xmax: " << px_xmax << " py_ymin: " << py_ymin << " py_ymax: " << py_ymax ); // ------------------------------------------------------------------------------------------------------- // Change the frame of reference from the robot to the camera // Create an array of all the x value options const int x_values_a[] = {px_xmax, px_ymax, px_xmin, px_ymin}; const int y_values_a[] = {py_xmax, py_ymax, py_xmin, py_ymin}; // Turn it into a vector std::vector<int> x_values (x_values_a, x_values_a + sizeof(x_values_a) / sizeof(x_values_a[0])); std::vector<int> y_values (y_values_a, y_values_a + sizeof(y_values_a) / sizeof(y_values_a[0])); // Find the min int x1 = *std::min_element(x_values.begin(), x_values.end()); int y1 = *std::min_element(y_values.begin(), y_values.end()); // Find the max int x2 = *std::max_element(x_values.begin(), x_values.end()); int y2 = *std::max_element(y_values.begin(), y_values.end()); ROS_DEBUG_STREAM_NAMED("perception","x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2); // ------------------------------------------------------------------------------------------------------- // Expand the ROI by a fudge factor, if possible const int FUDGE_FACTOR = 5; // pixels if( x1 > FUDGE_FACTOR) x1 -= FUDGE_FACTOR; if( y1 > FUDGE_FACTOR ) y1 -= FUDGE_FACTOR; if( x2 < image_width - FUDGE_FACTOR ) x2 += FUDGE_FACTOR; if( y2 < image_height - FUDGE_FACTOR ) y2 += FUDGE_FACTOR; ROS_DEBUG_STREAM_NAMED("perception","After Fudge Factor - x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2); // ------------------------------------------------------------------------------------------------------- // Create ROI parameters // (x1,y1)---------------------- // | | // | ROI | // | | // |_____________________(x2,y2)| // Create Region of Interest int roi_width = x2 - x1; int roi_height = y2 - y1; cv::Rect region_of_interest = cv::Rect( x1, y1, roi_width, roi_height ); ROS_DEBUG_STREAM_NAMED("perception","ROI: x " << x1 << " -- y " << y1 << " -- height " << roi_height << " -- width " << roi_width ); // ------------------------------------------------------------------------------------------------------- // Find paramters of the block in pixel coordiantes int block_center_x = x1 + 0.5*roi_width; int block_center_y = y1 + 0.5*roi_height; int block_center_z = block_size / 2; // TODO: make this better const cv::Point block_center = cv::Point( block_center_x, block_center_y ); // ------------------------------------------------------------------------------------------------------- // Create a sub image of just the block cv::Point a1 = cv::Point(x1, y1); cv::Point a2 = cv::Point(x2, y2); cv::rectangle( output_image, a1, a2, cv::Scalar(0, 255, 255), 1, 8); // Crop image (doesn't actually copy the data) cropped_image = full_input_image_gray(region_of_interest); // ------------------------------------------------------------------------------------------------------- // Detect edges using canny ROS_INFO_STREAM_NAMED("perception","Detecting edges using canny"); // Find edges cv::Mat canny_output; cv::Canny( cropped_image, canny_output, canny_threshold, canny_threshold*2, 3 ); // Get mini window stats const int mini_width = canny_output.size.p[1]; const int mini_height = canny_output.size.p[0]; const cv::Size mini_size = canny_output.size(); const cv::Point mini_center = cv::Point( mini_width/2, mini_height/2 ); // Find contours vector<vector<cv::Point> > contours; vector<cv::Vec4i> hierarchy; cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); ROS_INFO_STREAM_NAMED("perception","Contours"); // Draw contours cv::Mat drawing = cv::Mat::zeros( mini_size, CV_8UC3 ); ROS_INFO_STREAM_NAMED("perception","Drawing contours"); // Find the largest contour for getting the angle double max_contour_length = 0; int max_contour_length_i; for( size_t i = 0; i< contours.size(); i++ ) { double contour_length = cv::arcLength( contours[i], false ); if( contour_length > max_contour_length ) { max_contour_length = contour_length; max_contour_length_i = i; } //ROS_DEBUG_STREAM_NAMED("perception","Contour length = " << contour_length << " of index " << max_contour_length_i); cv::Scalar color = cv::Scalar( (30 + i*10) % 255, (30 + i*10) % 255, (30 + i*10) % 255); cv::drawContours( drawing, contours, (int)i, color, 1, 8, hierarchy, 0, cv::Point() ); //drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset ) } // ------------------------------------------------------------------------------------------------------- // Copy largest contour to main image cv::Scalar color = cv::Scalar( 0, 255, 0 ); cv::drawContours( output_image, contours, (int)max_contour_length_i, color, 1, 8, hierarchy, 0, a1 ); //drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset ) // ------------------------------------------------------------------------------------------------------- // Copy largest contour to seperate image cv::Mat hough_input = cv::Mat::zeros( mini_size, CV_8UC1 ); cv::Mat hough_input_color; cv::Scalar hough_color = cv::Scalar( 200 ); cv::drawContours( hough_input, contours, (int)max_contour_length_i, hough_color, 1, 8, hierarchy, 0 ); cv::cvtColor(hough_input, hough_input_color, CV_GRAY2BGR); // ------------------------------------------------------------------------------------------------------- // Hough Transform cv::Mat hough_drawing = cv::Mat::zeros( mini_size, CV_8UC3 ); std::vector<cv::Vec4i> lines; ROS_DEBUG_STREAM_NAMED("perception","hough_rho " << hough_rho << " hough_theta " << hough_theta << " theta_converted " << (1/hough_theta)*CV_PI/180 << " hough_threshold " << hough_threshold << " hough_minLineLength " << hough_minLineLength << " hough_maxLineGap " << hough_maxLineGap ); cv::HoughLinesP(hough_input, lines, hough_rho, (1/hough_theta)*CV_PI/180, hough_threshold, hough_minLineLength, hough_maxLineGap); ROS_WARN_STREAM_NAMED("perception","Found " << lines.size() << " lines"); std::vector<double> line_angles; // Copy detected lines to the drawing image for( size_t i = 0; i < lines.size(); i++ ) { cv::Vec4i line = lines[i]; cv::line( hough_drawing, cv::Point(line[0], line[1]), cv::Point(line[2], line[3]), cv::Scalar(255,255,255), 1, CV_AA); // Error check if(line[3] - line[1] == 0 && line[2] - line[0] == 0) { ROS_ERROR_STREAM_NAMED("perception","Line is actually two points at the origin, unable to calculate. TODO: handle better?"); continue; } // Find angle double line_angle = atan2(line[3] - line[1], line[2] - line[0]); //in radian, degrees: * 180.0 / CV_PI; // Reverse angle direction if negative if( line_angle < 0 ) { line_angle += CV_PI; } line_angles.push_back(line_angle); ROS_DEBUG_STREAM_NAMED("perception","Hough Line angle: " << line_angle * 180.0 / CV_PI;); } double block_angle = 0; // the overall result of the block's angle // Everything is based on the first angle if( line_angles.size() == 0 ) // make sure we have at least 1 angle { ROS_ERROR_STREAM_NAMED("perception","No lines were found for this cluster, unable to calculate block angle"); } else { calculateBlockAngle( line_angles, block_angle ); } // ------------------------------------------------------------------------------------------------------- // Draw chosen angle ROS_INFO_STREAM_NAMED("perception","Using block angle " << block_angle*180.0/CV_PI); // Draw chosen angle on mini image cv::Mat angle_drawing = cv::Mat::zeros( mini_size, CV_8UC3 ); int line_length = 0.5*double(mini_width); // have the line go 1/4 across the screen int new_x = mini_center.x + line_length*cos( block_angle ); int new_y = mini_center.y + line_length*sin( block_angle ); ROS_INFO_STREAM("Origin (" << mini_center.x << "," << mini_center.y << ") New (" << new_x << "," << new_y << ") length " << line_length << " angle " << block_angle << " mini width " << mini_width << " mini height " << mini_height); cv::Point angle_point = cv::Point(new_x, new_y); cv::line( angle_drawing, mini_center, angle_point, cv::Scalar(255,255,255), 1, CV_AA); // Draw chosen angle on contours image cv::line( hough_drawing, mini_center, angle_point, cv::Scalar(255,0, 255), 1, CV_AA); // Draw chosen angle on main image line_length = 0.75 * double(mini_width); // have the line go 1/2 across the box new_x = block_center_x + line_length*cos( block_angle ); new_y = block_center_y + line_length*sin( block_angle ); ROS_INFO_STREAM_NAMED("perception",block_center_x << ", " << block_center_y << ", " << new_x << ", " << new_y); angle_point = cv::Point(new_x, new_y); cv::line( output_image, block_center, angle_point, cv::Scalar(255,0,255), 2, CV_AA); // ------------------------------------------------------------------------------------------------------- // Get world coordinates // Find the block's center point double world_x1 = xmin+(xside)/2.0; double world_y1 = ymin+(yside)/2.0; double world_z1 = table_height + block_size / 2; // Convert pixel coordiantes back to world coordinates double world_x2 = cloud_transformed->at(new_x, new_y).x; double world_y2 = cloud_transformed->at(new_x, new_y).y; double world_z2 = world_z1; // is this even necessary? // Get angle from two world coordinates... double world_theta = abs( atan2(world_y2 - world_y1, world_x2 - world_x1) ); // Attempt to make all angles point in same direction makeAnglesUniform( world_theta ); // ------------------------------------------------------------------------------------------------------- // GUI Stuff // Copy the cluster image to the main image in the top left corner if( top_image_overlay_x + mini_width < image_width ) { const int common_height = 42; cv::Rect small_roi_row0 = cv::Rect(top_image_overlay_x, common_height*0, mini_width, mini_height); cv::Rect small_roi_row1 = cv::Rect(top_image_overlay_x, common_height*1, mini_width, mini_height); cv::Rect small_roi_row2 = cv::Rect(top_image_overlay_x, common_height*2, mini_width, mini_height); cv::Rect small_roi_row3 = cv::Rect(top_image_overlay_x, common_height*3, mini_width, mini_height); drawing.copyTo( output_image(small_roi_row0) ); hough_input_color.copyTo( output_image(small_roi_row1) ); hough_drawing.copyTo( output_image(small_roi_row2) ); angle_drawing.copyTo( output_image(small_roi_row3) ); top_image_overlay_x += mini_width; } // figure out the position and the orientation of the block //double angle = atan(block_size/((xside+yside)/2)); //double angle = atan( (xmaxy - xminy) / (xmax - xmin ) ); // Then add it to our set //addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle); //ROS_INFO_STREAM_NAMED("perception","FOUND -> xside: " << xside << " yside: " << yside << " angle: " << block_angle); addBlock( world_x1, world_y1, world_z1, world_theta ); //addBlock( block_center_x, block_center_y, block_center_z, block_angle); } else {
void OPTCalibration::perform() { const ViewMap & view_map = view_map_vec_.back(); if (initialization_) { if (not tree_initialized_ and not view_map.empty()) // Set /world { ViewMap::const_iterator it = view_map.begin(); while (it != view_map.end() and it->first->type() == TreeNode::DEPTH) ++it; if (it == view_map.end()) { view_map_vec_.resize(view_map_vec_.size() - 1); return; } TreeNode::Ptr tree_node = it->first; tree_node->sensor()->setParent(world_); tree_node->setLevel(0); ROS_INFO_STREAM(tree_node->sensor()->frameId() << " added to the tree."); tree_initialized_ = true; } if (view_map.empty()) { view_map_vec_.resize(view_map_vec_.size() - 1); // Remove data } else if (view_map.size() >= 2) // At least 2 cameras { int min_level = TreeNode::MAX_LEVEL; TreeNode::Ptr min_sensor_node; for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it) { TreeNode::Ptr tree_node = it->first; if (tree_node->level() < min_level) { min_level = tree_node->level(); min_sensor_node = tree_node; } } if (min_level < TreeNode::MAX_LEVEL) // At least one already in tree { for (ViewMap::const_iterator it = view_map.begin(); it != view_map.end(); ++it) { TreeNode::Ptr tree_node = it->first; if (tree_node != min_sensor_node) { const CheckerboardView::Ptr & view = it->second; cb::PlanarObject::Ptr planar_object = view->object; cb::Point3 center = view->center; if (tree_node->type() == TreeNode::INTENSITY) { cb::Scalar error = std::abs(planar_object->plane().normal().dot(cb::Vector3::UnitZ())) * center.squaredNorm(); if (tree_node->level() > min_level and error < tree_node->minError()) { CheckerboardView::Ptr min_checkerboard_view = view_map.at(min_sensor_node); cb::PlanarObject::Ptr min_planar_object = min_checkerboard_view->object; if (tree_node->level() == TreeNode::MAX_LEVEL) ROS_INFO_STREAM(tree_node->sensor()->frameId() << " added to the tree."); tree_node->setMinError(error); tree_node->setLevel(min_level + 1); tree_node->sensor()->setParent(min_sensor_node->sensor()); tree_node->sensor()->setPose(min_planar_object->pose() * planar_object->pose().inverse()); } } } } } initialization_ = (view_map_vec_.size() < OPTIMIZATION_COUNT); for (int i = 0; not initialization_ and i < node_vec_.size(); ++i) { TreeNode::Ptr & tree_node = node_vec_[i]; if (tree_node->type() == TreeNode::INTENSITY and tree_node->level() == TreeNode::MAX_LEVEL) initialization_ = true; } if (not initialization_) ROS_INFO("All cameras added to the tree. Now calibrate the global reference frame and save!"); } } else { if (view_map.empty()) { view_map_vec_.resize(view_map_vec_.size() - 1); // Remove data } else if (view_map.size() >= 2) // At least 2 cameras { if (last_optimization_ == 0) { optimize(); last_optimization_ = OPTIMIZATION_COUNT; } else last_optimization_--; } } }
void GlobalPlanner::Display() { m_lastDisplay = ros::Time::now(); ROS_INFO_STREAM("Robot Status:"); for (std::map<int, Robot_Ptr>::iterator it = m_robots.begin(); it != m_robots.end(); ++it) { ROS_INFO_STREAM(it->second->ToString()); } geometry_msgs::PoseArray availPoseArray; geometry_msgs::PoseArray finPoseArray; availPoseArray.header.stamp = ros::Time::now(); availPoseArray.header.frame_id = "/map"; finPoseArray.header = availPoseArray.header; std::map<int, Waypoint_Ptr> wps = m_tm.GetWaypoints(); if (wps.size() > 0) { ROS_INFO_STREAM("Waypoint Status:"); } for (std::map<int, Waypoint_Ptr>::iterator it = wps.begin(); it != wps.end(); ++it) { if (m_tm.IsWaypoint(it->first)) { ROS_INFO_STREAM(it->second->ToString()); if (it->second->GetStatus() == TaskResult::SUCCESS) finPoseArray.poses.push_back(it->second->GetPose()); else availPoseArray.poses.push_back(it->second->GetPose()); } } m_waypointPoseArrayAvailPub.publish(availPoseArray); m_waypointPoseArrayFinPub.publish(finPoseArray); finPoseArray.poses.clear(); availPoseArray.poses.clear(); std::map<int, Waypoint_Ptr> goals = m_tm.GetGoals(); if (goals.size() > 0) { ROS_INFO_STREAM("Goal Status"); } for (std::map<int, Waypoint_Ptr>::iterator it = goals.begin(); it != goals.end(); ++it) { ROS_INFO_STREAM(it->second->ToString()); if (it->second->GetStatus() == TaskResult::SUCCESS) finPoseArray.poses.push_back(it->second->GetPose()); else availPoseArray.poses.push_back(it->second->GetPose()); } m_goalPoseArrayAvailPub.publish(availPoseArray); m_goalPoseArrayFinPub.publish(finPoseArray); finPoseArray.poses.clear(); availPoseArray.poses.clear(); std::map<int, Dump_Ptr> dumps = m_tm.GetDumps(); if (dumps.size() > 0) { ROS_INFO_STREAM("Dump Status"); } for (std::map<int, Dump_Ptr>::iterator it = dumps.begin(); it != dumps.end(); ++it) { ROS_INFO_STREAM(it->second->ToString()); if (it->second->GetStatus() == TaskResult::SUCCESS) { finPoseArray.poses.push_back(it->second->GetPose1()); finPoseArray.poses.push_back(it->second->GetPose2()); } else { availPoseArray.poses.push_back(it->second->GetPose1()); availPoseArray.poses.push_back(it->second->GetPose2()); } } m_dumpPoseArrayAvailPub.publish(availPoseArray); m_dumpPoseArrayFinPub.publish(finPoseArray); }
int main(int argc, char** argv) { ros::init(argc, argv, "mtconnect_state_machine"); ros::NodeHandle ph("~"); ros::NodeHandle nh; std::string task_desc; std::map<std::string, trajectory_msgs::JointTrajectoryPtr> joint_paths; JointTractoryClientPtr joint_traj_client_ptr; control_msgs::FollowJointTrajectoryGoal joint_traj_goal; ros::ServiceClient trajectory_filter_client; arm_navigation_msgs::FilterJointTrajectoryWithConstraints trajectory_filter_; // Load parameters ROS_INFO_STREAM("Loading parameters"); if (!ph.getParam(PARAM_TASK_DESCRIPTION, task_desc)) { ROS_ERROR("Failed to load task description parameter"); return false; } std::map<std::string, boost::shared_ptr<mtconnect::JointPoint> > temp_pts; if (!mtconnect_state_machine::parseTaskXml(task_desc, joint_paths, temp_pts)) { ROS_ERROR("Failed to parse xml"); return false; } ROS_INFO_STREAM("Loaded " << joint_paths.size() << " paths"); // Load actions/clients joint_traj_client_ptr = JointTractoryClientPtr(new JointTractoryClient(DEFAULT_JOINT_TRAJ_ACTION, true)); trajectory_filter_client = nh.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>( DEFAULT_TRAJECTORY_FILTER_SERVICE); ROS_INFO_STREAM("Waiting for joint trajectory filter and action server"); joint_traj_client_ptr->waitForServer(ros::Duration(0)); trajectory_filter_client.waitForExistence(); ROS_INFO_STREAM("Starting loop through trajectory"); while (ros::ok()) { for(JTPMapItr itr = joint_paths.begin(); itr != joint_paths.end(); itr++) { ROS_INFO_STREAM("Loading " << itr->first << " path"); joint_traj_goal.trajectory = *itr->second; ROS_INFO_STREAM("Filtering a joint trajectory with " << joint_traj_goal.trajectory.points.size() << " points"); trajectory_filter_.request.trajectory = joint_traj_goal.trajectory; if (trajectory_filter_client.call(trajectory_filter_)) { if (trajectory_filter_.response.error_code.val == trajectory_filter_.response.error_code.SUCCESS) { ROS_INFO_STREAM("======================== MOVING ROBOT ========================"); ROS_INFO("Trajectory successfully filtered...sending goal"); joint_traj_goal.trajectory = trajectory_filter_.response.trajectory; ROS_INFO_STREAM("Sending a joint trajectory with " << joint_traj_goal.trajectory.points.size() << " points"); //joint_traj_client_ptr->sendGoal(joint_traj_goal); //joint_traj_client_ptr->waitForResult(ros::Duration(120)); joint_traj_client_ptr->sendGoalAndWait(joint_traj_goal, ros::Duration(120), ros::Duration(120)); } } else { ROS_ERROR("Failed to call filter trajectory"); } } } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "igv_TestNode"); ros::start(); ros::Rate loop_rate(10); uint8_t getTxLeft[12] = {0xa5, 0x3e, 0x02, 0x07, 0x00, 0x01, 0x19, 0xb6, 0xff, 0x00, 0x03, 0xff}; uint8_t enableBridgeLeft[12] = {0xa5, 0x3e, 0x02, 0x01, 0x00, 0x01, 0xab, 0x16, 0x00, 0x00, 0x00, 0x00}; uint8_t disableBridgeLeft[12] = {0xa5, 0x3e, 0x02, 0x01, 0x00, 0x01, 0xab, 0x16, 0x01, 0x00, 0x33, 0x31}; uint8_t setVelocityLeft[14] = {0xa5, 0x3e, 0x02, 0x45, 0x00, 0x02, 0x5a, 0x18, 0xb5, 0x01, 0x00, 0x00, 0x7a, 0xa4}; //======================================================================== uint8_t getTxRight[12] = {0xa5, 0x3f, 0x02, 0x07, 0x00, 0x01, 0xb3, 0xe7, 0xff, 0x00, 0x03, 0xff}; uint8_t enableBridgeRight[12] = {0xa5, 0x3f, 0x02, 0x01, 0x00, 0x01, 0x01, 0x47, 0x00, 0x00, 0x00, 0x00}; uint8_t disableBridgeRight[12] = {0xa5, 0x3f, 0x02, 0x01, 0x00, 0x01, 0x01, 0x47, 0x01, 0x00, 0x33, 0x31}; uint8_t setVelocityRight[14] = {0xa5, 0x3f, 0x02, 0x45, 0x00, 0x02, 0x96, 0x2b, 0xb5, 0x01, 0x00, 0x00, 0x7a, 0xa4}; //TODO Implement setting timeout to main program. serial::Serial LeftPort; LeftPort.setPort(LEFT_PORT); LeftPort.setBaudrate(115200); serial::Timeout to = serial::Timeout::simpleTimeout(1000); LeftPort.setTimeout(to); LeftPort.open(); ROS_INFO_STREAM("Left Port is opened."); // serial::Serial RightPort; // RightPort.setPort(RIGHT_PORT); // RightPort.setBaudrate(115200); // serial::Timeout to = serial::Timeout::simpleTimeout(1000); // RightPort.setTimeout(to); // RightPort.open(); // ROS_INFO_STREAM("Right Port is opened."); if(ros::ok()) { ros::spinOnce(); LeftPort.write(getTxLeft, 12); ROS_INFO_STREAM("Tx Access Got."); LeftPort.write(setVelocityLeft, 14); ROS_INFO_STREAM("Velocity Set."); LeftPort.write(enableBridgeLeft, 12); ROS_INFO_STREAM("Bridge Enabled."); ros::Duration(2).sleep(); LeftPort.write(disableBridgeLeft, 12); ROS_INFO_STREAM("Bridge Disabled."); // RightPort.write(getTxRight, 12); // ROS_INFO_STREAM("Tx Access Got."); // RightPort.write(setVelocityRight, 14); // ROS_INFO_STREAM("Velocity Set."); // RightPort.write(enableBridgeRight, 12); // ROS_INFO_STREAM("Bridge Enabled."); // ros::Duration(2).sleep(); // RightPort.write(disableBridgeRight, 12); // ROS_INFO_STREAM("Bridge Disabled."); loop_rate.sleep(); } ros::shutdown(); return 0; }
bool PlaceObject::IncrementalMoveTCP(const double threshold, const double eef_step, const double max_distance, const double timeout) { ROS_INFO("PlaceObject: Start incremental move in TCP frame."); move_group_->setMaxVelocityScalingFactor(0.001); ros::Time start = ros::Time::now(); bool collision = false; geometry_msgs::PoseStamped current_pose = move_group_->getCurrentPose(); geometry_msgs::PoseStamped goal_pose; goal_pose.header = current_pose.header; goal_pose.pose = target_pose_; goal_pose.pose.position.z -= 0.003; std::vector<geometry_msgs::Pose> waypoints; waypoints.push_back(current_pose.pose); waypoints.push_back(goal_pose.pose); robot_state::RobotState rs = *move_group_->getCurrentState(); move_group_->setStartState(rs); // Plan trajectory. moveit_msgs::RobotTrajectory trajectory; moveit_msgs::MoveItErrorCodes error_code; double path_fraction = move_group_->computeCartesianPath(waypoints, eef_step, 0.0, trajectory, false, &error_code); robot_trajectory::RobotTrajectory robot_trajectory(rs.getRobotModel(), move_group_->getName()); robot_trajectory.setRobotTrajectoryMsg(rs, trajectory); trajectory_processing::IterativeParabolicTimeParameterization iptp; iptp.computeTimeStamps(robot_trajectory, 1.0); robot_trajectory.getRobotTrajectoryMsg(trajectory); moveit::planning_interface::MoveGroup::Plan my_plan; moveit_msgs::RobotState robot_state_msg; robot_state::robotStateToRobotStateMsg(*move_group_->getCurrentState(), robot_state_msg); my_plan.trajectory_ = trajectory; my_plan.start_state_ = robot_state_msg; // removing points with velocity zero my_plan.trajectory_.joint_trajectory.points.erase( std::remove_if(my_plan.trajectory_.joint_trajectory.points.begin() + 1, my_plan.trajectory_.joint_trajectory.points.end(), [](const trajectory_msgs::JointTrajectoryPoint & p) { return p.time_from_start.toSec() == 0;}), my_plan.trajectory_.joint_trajectory.points.end()); // start sampling if (!gripper_interface_->ClientHandCommand("sampling")) { ROS_WARN("Execute Grasping: sampling failed"); return false; } int8_t collision_check = gripper_interface_->GetHandState().collision; printf("pre collision result: %d \n", collision_check); // if(collision_check) { // ROS_WARN("collision is already detected!"); // return true; // } // ros::Duration(0.5).sleep(); // removing points with velocity zero my_plan.trajectory_.joint_trajectory.points.erase( std::remove_if(my_plan.trajectory_.joint_trajectory.points.begin() + 1, my_plan.trajectory_.joint_trajectory.points.end(), [](const trajectory_msgs::JointTrajectoryPoint & p) { return p.time_from_start.toSec() == 0;}), my_plan.trajectory_.joint_trajectory.points.end()); move_group_->execute(my_plan); ROS_INFO_STREAM("robot state: effort: " << move_group_->getCurrentState()->hasVelocities()); while(move_group_->getCurrentState()->hasVelocities() ){ ROS_INFO_STREAM("robot state: effort: " << move_group_->getCurrentState()->hasEffort()); collision_check = gripper_interface_->GetHandState().collision; printf("%d \n", collision_check); if ((ros::Time::now() - start).toSec() > timeout) { ROS_WARN("Incremental movement: Timeout."); // move_group_->stop(); gripper_interface_->ClientHandCommand("c-open"); move_group_->stop(); return true; } ros::spinOnce(); ros::Duration(0.1).sleep(); } gripper_interface_->ClientHandCommand("c-open"); move_group_->stop(); ROS_WARN("collision was detected!"); return true; }
bool parseimumsg(uint8 * dataPacket, sensor_msgs::Imu * imu_msg) { float m1=0.0,m2=0.0,m3=0.0; // if(dataPacket[0] == 0x55 && dataPacket[1] == 0xaa) { //检验校验和 unsigned int sum = 0; for(int i = 0; i < PKGSIZE - 1; i++) { sum += dataPacket[i]; } //std::cout<< sum << " " << imumsg[14] << std::endl; // if( sum % 256 == dataPacket[PKGSIZE - 1]) { //解算加速度X,Y,Z int16 AccX = Byte16(int16, dataPacket[2], dataPacket[3]); int16 AccY = Byte16(int16, dataPacket[4], dataPacket[5]); int16 AccZ = Byte16(int16, dataPacket[6], dataPacket[7]); float Accscalar = MPU9250A_4g * GRAVITY_MSS; float AccTrueX = AccX * Accscalar; float AccTrueY = AccY * Accscalar; float AccTrueZ = AccZ * Accscalar; //解算角速度X,Y,Z int16 GyrX = Byte16(int16, dataPacket[8], dataPacket[9]); int16 GyrY = Byte16(int16, dataPacket[10], dataPacket[11]); int16 GyrZ = Byte16(int16, dataPacket[12], dataPacket[13]); float Gyrscalar = UserGyrResolution / 57.3; float GyrTrueX = GyrX * Gyrscalar; float GyrTrueY = GyrY * Gyrscalar; float GyrTrueZ = GyrZ * Gyrscalar; //解算Mag X,Y,Z int16 MagX = Byte16(int16, dataPacket[15], dataPacket[14]); int16 MagY = Byte16(int16, dataPacket[17], dataPacket[16]); int16 MagZ = Byte16(int16, dataPacket[19], dataPacket[18]); float Magscalar = MPU9250M_4800uT*Tesla2Gauss; float magx=MagX*Magscalar; float magy=MagY*Magscalar; float magz=MagZ*Magscalar; m1=magx+mag_offset[0]; m2=magy+mag_offset[1]; m3=magz+mag_offset[2]; MagTrueY=mag_sca[0]*m1+mag_sca[1]*m2+mag_sca[2]*m3; MagTrueX=mag_sca[3]*m1+mag_sca[4]*m2+mag_sca[5]*m3; MagTrueZ=-1.0*(mag_sca[6]*m1+mag_sca[7]*m2+mag_sca[8]*m3); newDataMag =(bool) dataPacket[20]; //发送Imu数据 imu_msg->header.stamp = ros::Time::now(); imu_msg->header.frame_id = "imu"; imu_msg->angular_velocity.x = GyrTrueX; imu_msg->angular_velocity.y = GyrTrueY; imu_msg->angular_velocity.z = GyrTrueZ; imu_msg->linear_acceleration.x = AccTrueX; imu_msg->linear_acceleration.y = AccTrueY; imu_msg->linear_acceleration.z = AccTrueZ; return true; } else { ROS_INFO_STREAM("Checksum Error"); return false; } } else { ROS_INFO_STREAM("Header Error"); return false; } }
void Action::setTolerance(const double value) { move_group_->setGoalTolerance(value); if (verbose_) ROS_INFO_STREAM("The GoalPositionTolerance = " << move_group_->getGoalPositionTolerance()); }
int main(int argc, char** argv) { ros::init(argc, argv, "collision_proximity_test"); ros::NodeHandle nh; std::string robot_description_name = nh.resolveName("robot_description", true); srand ( time(NULL) ); // initialize random seed ros::service::waitForService(ARM_QUERY_NAME); ros::ServiceClient query_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>(ARM_QUERY_NAME); kinematics_msgs::GetKinematicSolverInfo::Request request; kinematics_msgs::GetKinematicSolverInfo::Response response; query_client.call(request, response); int num_joints; std::vector<double> min_limits, max_limits; num_joints = (int) response.kinematic_solver_info.joint_names.size(); std::vector<std::string> joint_state_names = response.kinematic_solver_info.joint_names; std::map<std::string, double> joint_state_map; for(int i=0; i< num_joints; i++) { joint_state_map[joint_state_names[i]] = 0.0; min_limits.push_back(response.kinematic_solver_info.limits[i].min_position); max_limits.push_back(response.kinematic_solver_info.limits[i].max_position); } std::vector<std::vector<double> > valid_joint_states; valid_joint_states.resize(TEST_NUM); for(unsigned int i = 0; i < TEST_NUM; i++) { std::vector<double> jv(7,0.0); for(unsigned int j=0; j < min_limits.size(); j++) { jv[j] = gen_rand(std::max(min_limits[j],-M_PI),std::min(max_limits[j],M_PI)); } valid_joint_states[i] = jv; } collision_proximity::CollisionProximitySpace* cps = new collision_proximity::CollisionProximitySpace(robot_description_name); ros::ServiceClient planning_scene_client = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(planning_scene_name, true); ros::service::waitForService(planning_scene_name); arm_navigation_msgs::GetPlanningScene::Request ps_req; arm_navigation_msgs::GetPlanningScene::Response ps_res; arm_navigation_msgs::CollisionObject obj1; obj1.header.stamp = ros::Time::now(); obj1.header.frame_id = "odom_combined"; obj1.id = "obj1"; obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; obj1.shapes.resize(1); obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX; obj1.shapes[0].dimensions.resize(3); obj1.shapes[0].dimensions[0] = .1; obj1.shapes[0].dimensions[1] = .1; obj1.shapes[0].dimensions[2] = .75; obj1.poses.resize(1); obj1.poses[0].position.x = .6; obj1.poses[0].position.y = -.6; obj1.poses[0].position.z = .375; obj1.poses[0].orientation.w = 1.0; arm_navigation_msgs::AttachedCollisionObject att_obj; att_obj.object = obj1; att_obj.object.header.stamp = ros::Time::now(); att_obj.object.header.frame_id = "r_gripper_palm_link"; att_obj.link_name = "r_gripper_palm_link"; att_obj.touch_links.push_back("r_gripper_palm_link"); att_obj.touch_links.push_back("r_gripper_r_finger_link"); att_obj.touch_links.push_back("r_gripper_l_finger_link"); att_obj.touch_links.push_back("r_gripper_r_finger_tip_link"); att_obj.touch_links.push_back("r_gripper_l_finger_tip_link"); att_obj.touch_links.push_back("r_wrist_roll_link"); att_obj.touch_links.push_back("r_wrist_flex_link"); att_obj.touch_links.push_back("r_forearm_link"); att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link"); att_obj.object.id = "obj2"; att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER; att_obj.object.shapes[0].dimensions.resize(2); att_obj.object.shapes[0].dimensions[0] = .025; att_obj.object.shapes[0].dimensions[1] = .5; att_obj.object.poses.resize(1); att_obj.object.poses[0].position.x = .12; att_obj.object.poses[0].position.y = 0.0; att_obj.object.poses[0].position.z = 0.0; att_obj.object.poses[0].orientation.w = 1.0; ps_req.collision_object_diffs.push_back(obj1); ps_req.attached_collision_object_diffs.push_back(att_obj); arm_navigation_msgs::GetRobotState::Request rob_state_req; arm_navigation_msgs::GetRobotState::Response rob_state_res; ros::Rate slow_wait(1.0); while(1) { if(!nh.ok()) { ros::shutdown(); exit(0); } planning_scene_client.call(ps_req,ps_res); ROS_INFO_STREAM("Num coll " << ps_res.all_collision_objects.size() << " att " << ps_res.all_attached_collision_objects.size()); if(ps_res.all_collision_objects.size() > 0 && ps_res.all_attached_collision_objects.size() > 0) break; slow_wait.sleep(); } ROS_INFO("After test"); ros::ServiceClient robot_state_service = nh.serviceClient<arm_navigation_msgs::GetRobotState>(robot_state_name, true); ros::service::waitForService(robot_state_name); planning_models::KinematicState* state = cps->setupForGroupQueries("right_arm_and_end_effector", ps_res.complete_robot_state, ps_res.allowed_collision_matrix, ps_res.transformed_allowed_contacts, ps_res.all_link_padding, ps_res.all_collision_objects, ps_res.all_attached_collision_objects, ps_res.unmasked_collision_map); ros::WallDuration tot_ode, tot_prox; ros::WallDuration min_ode(1000.0,1000.0); ros::WallDuration min_prox(1000.0, 1000.0); ros::WallDuration max_ode; ros::WallDuration max_prox; std::vector<double> link_distances; std::vector<std::vector<double> > distances; std::vector<std::vector<tf::Vector3> > gradients; std::vector<std::string> link_names; std::vector<std::string> attached_body_names; std::vector<collision_proximity::CollisionType> collisions; unsigned int prox_num_in_collision = 0; unsigned int ode_num_in_collision = 0; ros::WallTime n1, n2; for(unsigned int i = 0; i < TEST_NUM; i++) { for(unsigned int j = 0; j < joint_state_names.size(); j++) { joint_state_map[joint_state_names[j]] = valid_joint_states[i][j]; } state->setKinematicState(joint_state_map); n1 = ros::WallTime::now(); cps->setCurrentGroupState(*state); bool in_prox_collision = cps->isStateInCollision(); n2 = ros::WallTime::now(); if(in_prox_collision) { prox_num_in_collision++; } ros::WallDuration prox_dur(n2-n1); if(prox_dur > max_prox) { max_prox = prox_dur; } else if (prox_dur < min_prox) { min_prox = prox_dur; } tot_prox += prox_dur; n1 = ros::WallTime::now(); bool ode_in_collision = cps->getCollisionModels()->isKinematicStateInCollision(*state); n2 = ros::WallTime::now(); if(ode_in_collision) { ode_num_in_collision++; } if(0){//in_prox_collision && !ode_in_collision) { ros::Rate r(1.0); while(nh.ok()) { cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions); ROS_INFO("Prox not ode"); cps->visualizeDistanceField(); cps->visualizeCollisions(link_names, attached_body_names, collisions); cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion()); std::vector<std::string> objs = link_names; objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end()); cps->visualizeObjectSpheres(objs); //cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion()); r.sleep(); } exit(0); } if(0 && !in_prox_collision && ode_in_collision) { ros::Rate r(1.0); while(nh.ok()) { ROS_INFO("Ode not prox"); cps->visualizeDistanceField(); cps->getStateCollisions(link_names, attached_body_names, in_prox_collision, collisions); cps->visualizeCollisions(link_names, attached_body_names, collisions); cps->visualizeConvexMeshes(cps->getCollisionModels()->getGroupLinkUnion()); std::vector<std::string> objs = link_names; objs.insert(objs.end(), attached_body_names.begin(), attached_body_names.end()); cps->visualizeObjectSpheres(objs); r.sleep(); } //cps->visualizeVoxelizedLinks(collision_models->getGroupLinkUnion()); std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts; std::vector<collision_space::EnvironmentModel::Contact> contact; cps->getCollisionModels()->getCollisionSpace()->getCollisionContacts(allowed_contacts, contact, 10); for(unsigned int i = 0; i < contact.size(); i++) { std::string name1; std::string name2; if(contact[i].link1 != NULL) { if(contact[i].link1_attached_body_index != 0) { name1 = contact[i].link1->getAttachedBodyModels()[contact[i].link1_attached_body_index-1]->getName(); } else { name1 = contact[i].link1->getName(); } } if(contact[i].link2 != NULL) { if(contact[i].link2_attached_body_index != 0) { name2 = contact[i].link2->getAttachedBodyModels()[contact[i].link2_attached_body_index-1]->getName(); } else { name2 = contact[i].link2->getName(); } } else if (!contact[i].object_name.empty()) { name2 = contact[i].object_name; } ROS_INFO_STREAM("Contact " << i << " between " << name1 << " and " << name2); } exit(0); if(0) { std::vector<double> prox_link_distances; std::vector<std::vector<double> > prox_distances; std::vector<std::vector<tf::Vector3> > prox_gradients; std::vector<std::string> prox_link_names; std::vector<std::string> prox_attached_body_names; cps->getStateGradients(prox_link_names, prox_attached_body_names, prox_link_distances, prox_distances, prox_gradients); ROS_INFO_STREAM("Link size " << prox_link_names.size()); for(unsigned int i = 0; i < prox_link_names.size(); i++) { ROS_INFO_STREAM("Link " << prox_link_names[i] << " closest distance " << prox_link_distances[i]); } for(unsigned int i = 0; i < prox_attached_body_names.size(); i++) { ROS_INFO_STREAM("Attached body names " << prox_attached_body_names[i] << " closest distance " << prox_link_distances[i+prox_link_names.size()]); } exit(0); } } ros::WallDuration ode_dur(n2-n1); if(ode_dur > max_ode) { max_ode = ode_dur; } else if (ode_dur < min_ode) { min_ode = ode_dur; } tot_ode += ode_dur; } //ROS_INFO_STREAM("Setup prox " << tot_prox_setup.toSec()/(TEST_NUM*1.0) << " ode " << tot_ode_setup.toSec()/(TEST_NUM*1.0)); ROS_INFO_STREAM("Av prox time " << (tot_prox.toSec()/(TEST_NUM*1.0)) << " min " << min_prox.toSec() << " max " << max_prox.toSec() << " percent in coll " << (prox_num_in_collision*1.0)/(TEST_NUM*1.0)); ROS_INFO_STREAM("Av ode time " << (tot_ode.toSec()/(TEST_NUM*1.0)) << " min " << min_ode.toSec() << " max " << max_ode.toSec() << " percent in coll " << (ode_num_in_collision*1.0)/(TEST_NUM*1.0)); ros::shutdown(); delete cps; }
bool Action::placeAction(MetaBlock *block, const std::string surface_name) { if (verbose_) ROS_INFO_STREAM("Placing " << block->name << " at pose " << block->goal_pose); // Prevent collision with table if (!surface_name.empty()) move_group_->setSupportSurfaceName(surface_name); std::vector<moveit_msgs::PlaceLocation> place_locations; // Re-usable datastruct geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.frame_id = grasp_data_.base_link_; pose_stamped.header.stamp = ros::Time::now(); // Create 360 degrees of place location rotated around a center //for (double angle = 0; angle < 2*M_PI; angle += M_PI/2) //{ pose_stamped.pose = block->goal_pose; // Create new place location moveit_msgs::PlaceLocation place_loc; place_loc.place_pose = pose_stamped; // Approach moveit_msgs::GripperTranslation pre_place_approach; pre_place_approach.direction.header.stamp = ros::Time::now(); pre_place_approach.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel pre_place_approach.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested. pre_place_approach.direction.header.frame_id = grasp_data_.base_link_; pre_place_approach.direction.vector.x = 0; pre_place_approach.direction.vector.y = 0; pre_place_approach.direction.vector.z = 0.1; //-1 // Approach direction (negative z axis) // TODO: document this assumption place_loc.pre_place_approach = pre_place_approach; // Retreat /*moveit_msgs::GripperTranslation post_place_retreat(pre_place_approach); post_place_retreat.direction.vector.x = 0; post_place_retreat.direction.vector.y = 0; post_place_retreat.direction.vector.z = -1; //1 // Retreat direction (pos z axis) place_loc.post_place_retreat = post_place_retreat;*/ // Post place posture - use same as pre-grasp posture (the OPEN command) //ROS_INFO_STREAM("place_loc.post_place_retreat" << place_loc.post_place_retreat); place_loc.post_place_posture = grasp_data_.pre_grasp_posture_; //grasp_data_.grasp_posture_; place_locations.push_back(place_loc); //} move_group_->setPlannerId("RRTConnectkConfigDefault"); bool success = move_group_->place(block->name, place_locations); if (verbose_) { if (success) ROS_INFO_STREAM("Place success! \n\n"); else ROS_ERROR_STREAM_NAMED("simple_actions:","Place failed."); } return success; }
MoveItSimpleControllerManager() : node_handle_("~") { if (!node_handle_.hasParam("controller_list")) { ROS_ERROR_STREAM("MoveitSimpleControllerManager: No controller_list specified."); return; } XmlRpc::XmlRpcValue controller_list; node_handle_.getParam("controller_list", controller_list); if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("MoveitSimpleControllerManager: controller_list should be specified as an array"); return; } /* actually create each controller */ for (int i = 0 ; i < controller_list.size() ; ++i) { if (!controller_list[i].hasMember("name") || !controller_list[i].hasMember("joints")) { ROS_ERROR("MoveitSimpleControllerManager: Name and joints must be specifed for each controller"); continue; } try { std::string name = std::string(controller_list[i]["name"]); std::string action_ns; if (controller_list[i].hasMember("ns")) { /* TODO: this used to be called "ns", renaming to "action_ns" and will remove in the future */ action_ns = std::string(controller_list[i]["ns"]); ROS_WARN("MoveitSimpleControllerManager: use of 'ns' is deprecated, use 'action_ns' instead."); } else if (controller_list[i].hasMember("action_ns")) action_ns = std::string(controller_list[i]["action_ns"]); else ROS_WARN("MoveitSimpleControllerManager: please note that 'action_ns' no longer has a default value."); if (controller_list[i]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR_STREAM("MoveitSimpleControllerManager: The list of joints for controller " << name << " is not specified as an array"); continue; } if (!controller_list[i].hasMember("type")) { ROS_ERROR_STREAM("MoveitSimpleControllerManager: No type specified for controller " << name); continue; } std::string type = std::string(controller_list[i]["type"]); ActionBasedControllerHandleBasePtr new_handle; if ( type == "GripperCommand" ) { new_handle.reset(new GripperControllerHandle(name, action_ns)); if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected()) { if (controller_list[i].hasMember("parallel")) { if (controller_list[i]["joints"].size() != 2) { ROS_ERROR_STREAM("MoveItSimpleControllerManager: Parallel Gripper requires exactly two joints"); continue; } static_cast<GripperControllerHandle*>(new_handle.get())->setParallelJawGripper(controller_list[i]["joints"][0], controller_list[i]["joints"][1]); } else { if (controller_list[i].hasMember("command_joint")) static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["command_joint"]); else static_cast<GripperControllerHandle*>(new_handle.get())->setCommandJoint(controller_list[i]["joints"][0]); } if (controller_list[i].hasMember("allow_failure")) static_cast<GripperControllerHandle*>(new_handle.get())->allowFailure(true); ROS_INFO_STREAM("MoveitSimpleControllerManager: Added GripperCommand controller for " << name ); controllers_[name] = new_handle; } } else if ( type == "FollowJointTrajectory" ) { new_handle.reset(new FollowJointTrajectoryControllerHandle(name, action_ns)); if (static_cast<FollowJointTrajectoryControllerHandle*>(new_handle.get())->isConnected()) { ROS_INFO_STREAM("MoveitSimpleControllerManager: Added FollowJointTrajectory controller for " << name ); controllers_[name] = new_handle; } } else { ROS_ERROR("Unknown controller type: '%s'", type.c_str()); continue; } if (!controllers_[name]) { controllers_.erase(name); continue; } /* add list of joints, used by controller manager and moveit */ for (int j = 0 ; j < controller_list[i]["joints"].size() ; ++j) controllers_[name]->addJoint(std::string(controller_list[i]["joints"][j])); } catch (...) { ROS_ERROR("MoveitSimpleControllerManager: Unable to parse controller information"); } } }
void BasicWorker::MsgCB(const std_msgs::Int32ConstPtr &msg) { ROS_INFO("Got callback"); ROS_INFO_STREAM("Heard: " << msg->data); }
int main(int argc, char** argv) { bool desired_mode_full=false; //if false go in safe_mode bool ir_warning_; bool bumper_warning_; ros::init(argc, argv, "roomba560_node"); ROS_INFO("Roomba for ROS %.2f", NODE_VERSION); double last_x, last_y, last_yaw; double vel_x, vel_y, vel_yaw; double dt; float last_charge = 0.0; int time_remaining = -1; ros::NodeHandle n; ros::NodeHandle pn("~"); std::string base_name; int id; pn.param<std::string>("base_name_", base_name, "iRobot_"); pn.param<int>("id_", id, 0); std_msgs::String my_namespace; my_namespace.data = base_name + std::to_string(id); bool publish_name; ros::Publisher listpub; pn.param<bool>("publish_name_", publish_name, false); if(publish_name) { listpub = n.advertise<std_msgs::String>("/agent_list", 50); } int name_count=0; pn.param<std::string>("port_", port, "/dev/ttyUSB0"); std::string base_frame_id; std::string odom_frame_id; pn.param<std::string>("base_frame_id", base_frame_id, my_namespace.data + "/base_link"); pn.param<std::string>("odom_frame_id", odom_frame_id, my_namespace.data + "/odom"); bool publishTf; pn.param<bool>("publishTf", publishTf, true); Eigen::MatrixXd poseCovariance(6,6); Eigen::MatrixXd twistCovariance(6,6); bool pose_cov_mat = false; bool twist_cov_mat = false; std::vector<double> pose_covariance_matrix; std::vector<double> twist_covariance_matrix; XmlRpc::XmlRpcValue poseCovarConfig; XmlRpc::XmlRpcValue twistCovarConfig; if (pn.hasParam("poseCovariance")) { try { pn.getParam("poseCovariance", poseCovarConfig); ROS_ASSERT(poseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray); int matSize = poseCovariance.rows(); if (poseCovarConfig.size() != matSize * matSize) { ROS_WARN_STREAM("Pose_covariance matrix should have " << matSize * matSize << " values."); } else { for (int i = 0; i < matSize; i++) { for (int j = 0; j < matSize; j++) { std::ostringstream ostr; ostr << poseCovarConfig[matSize * i + j]; std::istringstream istr(ostr.str()); istr >> poseCovariance(i, j); pose_covariance_matrix.push_back(poseCovariance(i,j)); } } pose_cov_mat = true; } } catch (XmlRpc::XmlRpcException &e) { ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for pose_covariance (type: " << poseCovarConfig.getType() << ")"); } } if (pn.hasParam("twistCovariance")) { try { pn.getParam("twistCovariance", twistCovarConfig); ROS_ASSERT(twistCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray); int matSize = twistCovariance.rows(); if (twistCovarConfig.size() != matSize * matSize) { ROS_WARN_STREAM("Twist_covariance matrix should have " << matSize * matSize << " values."); } else { for (int i = 0; i < matSize; i++) { for (int j = 0; j < matSize; j++) { std::ostringstream ostr; ostr << twistCovarConfig[matSize * i + j]; std::istringstream istr(ostr.str()); istr >> twistCovariance(i, j); twist_covariance_matrix.push_back(twistCovariance(i,j)); } } twist_cov_mat = true; } } catch (XmlRpc::XmlRpcException &e) { ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for twist_covariance (type: " << poseCovarConfig.getType() << ")"); } } roomba = new irobot::OpenInterface(port.c_str()); ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50); ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("battery", 50); ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("bumper", 50); ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("buttons", 50); ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("cliff", 50); ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("ir_bumper", 50); ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("ir_character", 50); ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("wheel_drop", 50); tf::TransformBroadcaster tf_broadcaster; ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmdVelReceived); ros::Subscriber leds_sub = n.subscribe<irobotcreate2::Leds>("leds", 1, ledsReceived); ros::Subscriber digitleds_sub = n.subscribe<irobotcreate2::DigitLeds>("digit_leds", 1, digitLedsReceived); ros::Subscriber song_sub = n.subscribe<irobotcreate2::Song>("song", 1, songReceived); ros::Subscriber playsong_sub = n.subscribe<irobotcreate2::PlaySong>("play_song", 1, playSongReceived); ros::Subscriber mode_sub = n.subscribe<std_msgs::String>("mode", 1, cmdModeReceived); ros::Subscriber special_sub = n.subscribe<std_msgs::String>("special", 1, cmdSpecialReceived); irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100}; roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE); if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba."); else { ROS_FATAL("Could not connect to Roomba."); ROS_BREAK(); } ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); bool first_loop=true; while(roomba->getSensorPackets(100) == -1 && ros::ok()) { usleep(100); ROS_INFO_STREAM("Waiting for roomba sensors"); } roomba->calculateOdometry(); roomba->resetOdometry(); ros::Rate r(50.0); while(n.ok()) { if(publish_name) { name_count++; if(name_count >= 10) { listpub.publish(my_namespace); name_count=0; } } ir_warning_ = false; bumper_warning_ = false; current_time = ros::Time::now(); last_x = roomba->odometry_x_; last_y = roomba->odometry_y_; last_yaw = roomba->odometry_yaw_; if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets."); else roomba->calculateOdometry(); /* OLD CODE * dt = (current_time - last_time).toSec(); vel_x = (roomba->odometry_x_ - last_x)/dt; vel_y = (roomba->odometry_y_ - last_y)/dt; vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;*/ vel_x = roomba->new_odometry_.getLinear(); vel_y = 0.0; vel_yaw = roomba->new_odometry_.getAngular(); // ****************************************************************************************** //first, we'll publish the transforms over tf if(publishTf) { geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = current_time; odom_trans.header.frame_id = odom_frame_id; odom_trans.child_frame_id = base_frame_id; odom_trans.transform.translation.x = roomba->odometry_x_; odom_trans.transform.translation.y = roomba->odometry_y_; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_); tf_broadcaster.sendTransform(odom_trans); } //TODO: Finish brodcasting the tf for all the ir sensors on the Roomba /*geometry_msgs::TransformStamped cliff_left_trans; cliff_left_trans.header.stamp = current_time; cliff_left_trans.header.frame_id = "base_link"; cliff_left_trans.child_frame_id = "base_cliff_left"; cliff_left_trans.transform.translation.x = 0.0; cliff_left_trans.transform.translation.y = 0.0; cliff_left_trans.transform.translation.z = 0.0; cliff_left_trans.transform.rotation = ; tf_broadcaster.sendTransform(cliff_left_trans); */ // ****************************************************************************************** //next, we'll publish the odometry message over ROS nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = odom_frame_id; //set the position odom.pose.pose.position.x = roomba->odometry_x_; odom.pose.pose.position.y = roomba->odometry_y_; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_); if(pose_cov_mat) for(int i = 0; i < pose_covariance_matrix.size(); i++) odom.pose.covariance[i] = pose_covariance_matrix[i]; //set the velocity odom.child_frame_id = base_frame_id; odom.twist.twist.linear.x = vel_x; odom.twist.twist.linear.y = vel_y; odom.twist.twist.angular.z = vel_yaw; if(twist_cov_mat) for(int i = 0; i < pose_covariance_matrix.size(); i++) odom.twist.covariance[i] = twist_covariance_matrix[i]; //publish the message odom_pub.publish(odom); // ****************************************************************************************** //publish battery irobotcreate2::Battery battery; battery.header.stamp = current_time; battery.power_cord = roomba->power_cord_; battery.dock = roomba->dock_; battery.level = 100.0*(roomba->charge_/roomba->capacity_); if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60; last_charge = roomba->charge_; battery.time_remaining = time_remaining; battery_pub.publish(battery); // ****************************************************************************************** //publish bumpers irobotcreate2::Bumper bumper; bumper.left.header.stamp = current_time; bumper.left.state = roomba->bumper_[LEFT]; bumper.right.header.stamp = current_time; bumper.right.state = roomba->bumper_[RIGHT]; bumper_pub.publish(bumper); bumper_warning_ = bumper.left.state || bumper.right.state; bumper_warning.store(bumper_warning_); // ****************************************************************************************** //publish buttons irobotcreate2::Buttons buttons; buttons.header.stamp = current_time; buttons.clean = roomba->buttons_[BUTTON_CLEAN]; buttons.spot = roomba->buttons_[BUTTON_SPOT]; buttons.dock = roomba->buttons_[BUTTON_DOCK]; buttons.day = roomba->buttons_[BUTTON_DAY]; buttons.hour = roomba->buttons_[BUTTON_HOUR]; buttons.minute = roomba->buttons_[BUTTON_MINUTE]; buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE]; buttons.clock = roomba->buttons_[BUTTON_CLOCK]; buttons_pub.publish(buttons); // ****************************************************************************************** //publish cliff irobotcreate2::RoombaIR cliff; cliff.header.stamp = current_time; cliff.header.frame_id = "base_cliff_left"; cliff.state = roomba->cliff_[LEFT]; cliff.signal = roomba->cliff_signal_[LEFT]; cliff_pub.publish(cliff); cliff.header.frame_id = "base_cliff_front_left"; cliff.state = roomba->cliff_[FRONT_LEFT]; cliff.signal = roomba->cliff_signal_[FRONT_LEFT]; cliff_pub.publish(cliff); cliff.header.frame_id = "base_cliff_front_right"; cliff.state = roomba->cliff_[FRONT_RIGHT]; cliff.signal = roomba->cliff_signal_[FRONT_RIGHT]; cliff_pub.publish(cliff); cliff.header.frame_id = "base_cliff_right"; cliff.state = roomba->cliff_[RIGHT]; cliff.signal = roomba->cliff_signal_[RIGHT]; cliff_pub.publish(cliff); // ****************************************************************************************** //publish irbumper irobotcreate2::RoombaIR irbumper; irbumper.header.stamp = current_time; irbumper.header.frame_id = "base_irbumper_left"; irbumper.state = roomba->ir_bumper_[LEFT]; irbumper.signal = roomba->ir_bumper_signal_[LEFT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; irbumper.header.frame_id = "base_irbumper_front_left"; irbumper.state = roomba->ir_bumper_[FRONT_LEFT]; irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; irbumper.header.frame_id = "base_irbumper_center_left"; irbumper.state = roomba->ir_bumper_[CENTER_LEFT]; irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; irbumper.header.frame_id = "base_irbumper_center_right"; irbumper.state = roomba->ir_bumper_[CENTER_RIGHT]; irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; irbumper.header.frame_id = "base_irbumper_front_right"; irbumper.state = roomba->ir_bumper_[FRONT_RIGHT]; irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; irbumper.header.frame_id = "base_irbumper_right"; irbumper.state = roomba->ir_bumper_[RIGHT]; irbumper.signal = roomba->ir_bumper_signal_[RIGHT]; irbumper_pub.publish(irbumper); ir_warning_ = ir_warning_ || irbumper.state; ir_warning.store(ir_warning_); // ****************************************************************************************** //publish irchar irobotcreate2::IRCharacter irchar; irchar.header.stamp = current_time; irchar.omni = roomba->ir_char_[OMNI]; irchar.left = roomba->ir_char_[LEFT]; irchar.right = roomba->ir_char_[RIGHT]; irchar_pub.publish(irchar); // ****************************************************************************************** //publish wheeldrop irobotcreate2::WheelDrop wheeldrop; wheeldrop.left.header.stamp = current_time; wheeldrop.left.state = roomba->wheel_drop_[LEFT]; wheeldrop.right.header.stamp = current_time; wheeldrop.right.state = roomba->wheel_drop_[RIGHT]; wheeldrop_pub.publish(wheeldrop); ros::spinOnce(); r.sleep(); if(first_loop) { roomba->startOI(true); if(!desired_mode_full) roomba->Safe(); first_loop=false; } } roomba->powerDown(); roomba->closeSerialPort(); }
// Call setup functions bool GlobalPlanner::Init(ros::NodeHandle* nh) { ROS_INFO_STREAM("Initializing global planner class"); m_nh = nh; SetupCallbacks(); ROS_INFO_STREAM("Initializing TaskMaster"); std::string waypointFile("testList1.points"); if (m_nh->getParam("/global_planner/waypoints_file", waypointFile)) { ROS_INFO_STREAM("Read from file: "<<waypointFile); } // Planner types : // naive : original, for each waypoint in order of file, choose first available robot // closest_robot : for each waypoint in order of file, choose closest available robot // closest_waypoint : for each available robot in order of id, choose closest available waypoint std::string planner("naive"); m_planner = PLANNER_NAIVE; if (m_nh->getParam("/global_planner/planner", planner)) { ROS_INFO_STREAM("Planner set from file to " << planner); if (planner.compare("closest_robot") == 0) { m_planner = PLANNER_CLOSEST_ROBOT; } else if (planner.compare("closest_waypoint") == 0) { m_planner = PLANNER_CLOSEST_WAYPOINT; } } // Load list of possible dumpsites // std::string dumpsiteFile("cubicle_dump_meets.points"); std::string dumpsiteFile("springdemo_dump_meets.points"); m_nh->getParam("/global_planner/dumpsite_file", dumpsiteFile); ROS_INFO_STREAM("Loading dumpsite file: " << dumpsiteFile); loadDumpSites(dumpsiteFile); // Need to wait for m_robots to get populated by callback // Some better ways todo: call globalplanner with param saying # of robots, then wait till # robots is populated // for now just pausing for 10 seconds // Unsustainable. Must hack harder. // ROS_INFO_STREAM("Waiting till 2 robot controllers are loaded and update global planner robot map..."); // int robot_count = 0; // while (robot_count < 2) // { // robot_count = 0; // for (std::map<int, Robot_Ptr>::iterator it = m_robots.begin(); it != m_robots.end(); ++it) // { // robot_count++; // } // ROS_INFO_STREAM(robot_count << " robots loaded so far."); // ros::spinOnce(); // sleep(1); // } // ROS_INFO("Done waiting, initializing task master."); m_tm.Init(nh, m_robots, waypointFile); ros::spinOnce(); return true; }
int main(int argc, char **argv) { std::string nodeName = "node"; std::string hostName = "localhost"; std::string configFile = ""; for(int i = 0; i < argc; i++) { if(!strcmp(argv[i], "-nodename")) nodeName = argv[i+1]; if(!strcmp(argv[i], "-hostname")) hostName = argv[i+1]; if(!strcmp(argv[i], "-config")) configFile = argv[i+1]; } std::vector<boost::thread*> compThreads; XMLParser nodeParser; std::string configFileName = nodeName + ".xml"; if (configFile.length() > 0) configFileName = configFile; if (nodeParser.Parse(configFileName)) { for (int i=0;i<nodeParser.libList.size();i++) { void *hndl = dlopen(nodeParser.libList[i].c_str(), RTLD_LAZY | RTLD_GLOBAL); if (hndl == NULL) { cerr << dlerror() << endl; exit(-1); } else ROS_INFO_STREAM("Opened " << nodeParser.libList[i]); } nodeName = nodeParser.nodeName; ros::init(argc, argv, nodeName.c_str()); // Create Node Handle ros::NodeHandle n; ROS_INFO_STREAM(nodeName << " thread id = " << boost::this_thread::get_id()); for (int i=0;i<nodeParser.compConfigList.size();i++) { std::string libraryLocation = nodeParser.compConfigList[i].libraryLocation; void *hndl = dlopen(libraryLocation.c_str(), RTLD_NOW); if(hndl == NULL) { cerr << dlerror() << endl; exit(-1); } void *mkr = dlsym(hndl, "maker"); Component *comp_inst = ((Component *(*)(ComponentConfig &, int , char **))(mkr)) (nodeParser.compConfigList[i], argc, argv); // Create Component Threads boost::thread *comp_thread = new boost::thread(componentThreadFunc, comp_inst); compThreads.push_back(comp_thread); ROS_INFO_STREAM(nodeName << " has started " << nodeParser.compConfigList[i].compName); } for (int i=0;i<compThreads.size();i++) { compThreads[i]->join(); } return 0; } else { printf("ERROR::Unable to parse XML file\n"); return -1; } }
// Executive function void GlobalPlanner::Execute() { ros::spinOnce(); if ((ros::Time::now() - m_lastDisplay) > ros::Duration(5)) { Display(); } // Get Robot Status... QueryRobots(); // FOR each pair of robots that're just chillin in a dump stage and are stopped near the handoff location // Pick best bot to meet with? // Check if robot1 & robot2 are within handoff threshold // Add new dump to m_dumpMap // Send handoff message to each robot giving them their new capacities // // IF robot in wait state AND is full AND there are goals // Find the best dump location for the robots to meet // Update Dump List // Send Dump Message // // IF there are goals available // pick best robot to do job (return an ID) // OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to // Send goal message // ELSE IF robots available AND waypoints available // pick best robot to get to waypoints (return an ID) // OPTIONAL: IF need to cancel robot's current goal -> Send Cancel Message First AND update the task it was assigned to // Send Waypoint Message // Check if a robot is full & find the best binbot for it std::vector<Robot_Ptr> availableRobots = GetAvailableRobots(0); // Get any available robots even with no storage space left for (std::vector<Robot_Ptr>::iterator it = availableRobots.begin(); it != availableRobots.end(); ++it) { Robot_Ptr robot = *it; // If robot has no space (TODO: Check if collector bot or bin bot for where to dump) if (robot->GetStorageAvailable() <= 0 && robot->GetType() == RobotState::COLLECTOR_BOT) { ROS_INFO_STREAM_THROTTLE(5, "Robot " << robot->GetName() << "(" << robot->GetID() << ")" << " full, searching for closest available bin bot..."); int collectorBot = robot->GetID(); // Get closest bin bot to collector bot if it exists int bestBinBot = GetBestBinBot( collectorBot ); if (bestBinBot != NO_ROBOT_FOUND) { ROS_INFO_STREAM("Bin Bot " << robot->GetName() << "(" << robot->GetID() << ") found."); // Create new dump site Dump_Ptr dp(new DumpWrapper(dumps_count++)); m_tm.AddDump(dp); // Assign collector robot to dump, Assign bin bot to dump if (!AssignRobotsDump(collectorBot, bestBinBot, dp->GetID())) { ROS_ERROR_STREAM("Error Assigning Robots " << m_robots[collectorBot]->GetName() << "(" << collectorBot << ") & " << m_robots[bestBinBot]->GetName() << "(" << bestBinBot << ")" << " to Dump(" << dp->GetID() << ")"); } } } } // Check each dump, see if each robot in the dump finished state std::map<int, Dump_Ptr > dumps = m_tm.GetDumps(); for (std::map<int, Dump_Ptr>::iterator it = dumps.begin(); it != dumps.end(); ++it) { Dump_Ptr dump = it->second; int collectorBot = dump->GetRobot1(); int binBot = dump->GetRobot2(); // if (dump->GetReadyToTransfer()) { // This requires services to work // Avoid services by querying robot states if ( dump->GetInProgress() && (m_robots[collectorBot]->GetState() == RobotState::WAITING || m_robots[binBot]->GetState() == RobotState::WAITING) ) { // Resend dump so waiting robot m_tm.SendDump(dump->GetID()); } if (dump->GetInProgress() && m_robots[collectorBot]->GetState() == RobotState::DUMPING_FINISHED && m_robots[binBot]->GetState() == RobotState::DUMPING_FINISHED) { ROS_INFO_STREAM("Dump(" << dump->GetID() << ") transferring trash."); // Move trash over int collectorBot = dump->GetRobot1(); int binBot = dump->GetRobot2(); int trash_to_transfer = m_robots[collectorBot]->GetStorageUsed(); // Send trash updates to robot controllers global_planner::SetTrashSrv s; s.request.storage = 0; if (m_setTrashServices[collectorBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[collectorBot]->GetName()); } else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << collectorBot); } s.request.storage = m_robots[binBot]->GetStorageUsed() + trash_to_transfer; if (m_setTrashServices[binBot].call(s) && s.response.result == 0) { ROS_INFO_STREAM("Set Trash for " << m_robots[binBot]->GetName()); } else { ROS_ERROR_STREAM("Did not receive trash response from robot: " << binBot); } // In global planner, update storage values // Add trash to bin bot m_robots[binBot]->SetStorageUsed( m_robots[binBot]->GetStorageUsed() + trash_to_transfer ); // Remove trash from collector bot m_robots[collectorBot]->SetStorageUsed(0); // Set states to waiting in global planner but not robot controllers // the robot controllers are in DUMP_FINISHED state, but they're ready to transition to waypoints etc. // So set them to waiting in global planner and hopefully go to planning before a callback update resets them. m_robots[collectorBot]->SetState(RobotState::WAITING); m_robots[binBot]->SetState(RobotState::WAITING); // Transition dump state to success dump->SetStatus(TaskResult::SUCCESS); } } // ProcessGoals(); /* // If no available waypoints, do nothing if (m_tm.GetAvailableWaypoints().size() == 0) { ROS_WARN_STREAM_THROTTLE(10, "No Available Waypoints! (" << m_tm.GetAvailableWaypoints().size() << ") " << (isFinished() ? "FIN" : "GP Waiting") ); // Print out status of all waypoints std::map<int, Waypoint_Ptr> allWaypoints = m_tm.GetWaypoints(); std::stringstream ss; for (std::map<int, Waypoint_Ptr>::iterator it = allWaypoints.begin(); it != allWaypoints.end(); ++it) { ss << it->second->GetID() << "-" << it->second->GetStatusMessage() << " "; } ROS_INFO_STREAM_THROTTLE(10, ss.str() ); return; } */ switch (m_planner) { case PLANNER_CLOSEST_ROBOT: PlanNNRobot(); break; case PLANNER_CLOSEST_WAYPOINT: PlanNNWaypoint(); break; case PLANNER_NAIVE: default: PlanNaive(); break; } }
void TonavRos::printHelp() { ROS_INFO_STREAM(options_description_); }
void NavSatTransform::computeTransform() { // Only do this if: // 1. We haven't computed the odom_frame->utm_frame transform before // 2. We've received the data we need if (!transformGood_ && hasTransformOdom_ && hasTransformGps_ && hasTransformImu_) { // Get the IMU's current RPY values. Need the raw values (for yaw, anyway). tf2::Matrix3x3 mat(transformOrientation_); // Convert to RPY double imuRoll; double imuPitch; double imuYaw; mat.getRPY(imuRoll, imuPitch, imuYaw); /* The IMU's heading was likely originally reported w.r.t. magnetic north. * However, all the nodes in robot_localization assume that orientation data, * including that reported by IMUs, is reported in an ENU frame, with a 0 yaw * value being reported when facing east and increasing counter-clockwise (i.e., * towards north). Conveniently, this aligns with the UTM grid, where X is east * and Y is north. However, we have two additional considerations: * 1. The IMU may have its non-ENU frame data transformed to ENU, but there's * a possibility that its data has not been corrected for magnetic * declination. We need to account for this. A positive magnetic * declination is counter-clockwise in an ENU frame. Therefore, if * we have a magnetic declination of N radians, then when the sensor * is facing a heading of N, it reports 0. Therefore, we need to add * the declination angle. * 2. To account for any other offsets that may not be accounted for by the * IMU driver or any interim processing node, we expose a yaw offset that * lets users work with navsat_transform_node. */ imuYaw += (magneticDeclination_ + yawOffset_); ROS_INFO_STREAM("Corrected for magnetic declination of " << std::fixed << magneticDeclination_ << " and user-specified offset of " << yawOffset_ << ". Transform heading factor is now " << imuYaw); // Convert to tf-friendly structures tf2::Quaternion imuQuat; imuQuat.setRPY(0.0, 0.0, imuYaw); // The transform order will be orig_odom_pos * orig_utm_pos_inverse * cur_utm_pos. // Doing it this way will allow us to cope with having non-zero odometry position // when we get our first GPS message. tf2::Transform utmPoseWithOrientation; utmPoseWithOrientation.setOrigin(transformUtmPose_.getOrigin()); utmPoseWithOrientation.setRotation(imuQuat); utmWorldTransform_.mult(transformWorldPose_, utmPoseWithOrientation.inverse()); utmWorldTransInverse_ = utmWorldTransform_.inverse(); double roll = 0; double pitch = 0; double yaw = 0; mat.setRotation(latestWorldPose_.getRotation()); mat.getRPY(roll, pitch, yaw); ROS_INFO_STREAM("Transform world frame pose is: " << std::fixed << "\nPosition: (" << transformWorldPose_.getOrigin().getX() << ", " << transformWorldPose_.getOrigin().getY() << ", " << transformWorldPose_.getOrigin().getZ() << ")" << "\nOrientation: (" << roll << ", " << pitch << ", " << yaw << ")"); mat.setRotation(utmWorldTransform_.getRotation()); mat.getRPY(roll, pitch, yaw); ROS_INFO_STREAM("World frame->utm transform is " << std::fixed << "\nPosition: (" << utmWorldTransform_.getOrigin().getX() << ", " << utmWorldTransform_.getOrigin().getY() << ", " << utmWorldTransform_.getOrigin().getZ() << ")" << "\nOrientation: (" << roll << ", " << pitch << ", " << yaw << ")"); transformGood_ = true; // Send out the (static) UTM transform in case anyone else would like to use it. if (broadcastUtmTransform_) { geometry_msgs::TransformStamped utmTransformStamped; utmTransformStamped.header.stamp = ros::Time::now(); utmTransformStamped.header.frame_id = worldFrameId_; utmTransformStamped.child_frame_id = "utm"; utmTransformStamped.transform = tf2::toMsg(utmWorldTransform_); utmBroadcaster_.sendTransform(utmTransformStamped); } } }