void PersonTracker::skeletonCB(const body_msgs::Skeletons& skel_msg) { //ROS_INFO_THROTTLE(5,"[person tracker] Got data, %lu skeletons.", skel_msg.skeletons.size()); body_msgs::SkeletonJoint body_part; string body_part_name; std::vector<double> thresholds; thresholds.push_back(0.7); //thresholds.push_back(0.5); //thresholds.push_back(0.3); //thresholds.push_back(-0.1); for( int ithresh = 0; ithresh<thresholds.size(); ithresh++ ) { // Find the first trackable person for( unsigned int i=0; i<skel_msg.skeletons.size(); i++ ) { // If the person has a trackable joint, save the location if( getFirstGoodJoint(skel_msg.skeletons.at(i), thresholds.at(ithresh), body_part, body_part_name) ) { ROS_INFO_THROTTLE(2,"[person tracker] Player %d's %s has confidence %f", skel_msg.skeletons.at(i).playerid, body_part_name.c_str(), body_part.confidence); // Create a new pose PoseWithCovarianceStamped temp_pose; temp_pose.pose.pose.position = body_part.position; temp_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); temp_pose.header.frame_id = skel_msg.header.frame_id; temp_pose.header.stamp = skel_msg.header.stamp; setVariance(temp_pose, 0.0); // Transform the goal to a global, fixed frame. PoseWithCovarianceStamped transformed_pose; if( poseToGlobalFrame(temp_pose, transformed_pose) ) { person_pos_ = transformed_pose; last_detect_ = ros::Time::now(); break; } } } } }
void Tracker::spinOnce() { if (started){ auto begin_time=std::chrono::high_resolution_clock::now(); track(); update_markers(); publish_markers(); broadcast_tf(); auto end_time=std::chrono::high_resolution_clock::now(); auto elapsed_time=std::chrono::duration_cast<std::chrono::milliseconds>(end_time - begin_time).count(); ROS_INFO_THROTTLE(10,"[Tracker::%s]\tStep time: %ld ms.",__func__,elapsed_time); } else if (lost_it){ find_object_in_scene(); } else{ if(!storage->readObjNames(est_names)) ROS_WARN_THROTTLE(30,"[Tracker::%s]\tLooks like no Pose Estimation has been performed, perform one in order to start using the object tracker.",__func__); } }
gm::Pose::ConstPtr Node::getBasePose (const ros::Time& t) { bool found = tf_.waitForTransform(fixed_frame_, base_frame_, t, processing_interval_); if (found) { try { tf::StampedTransform trans; tf_.lookupTransform(fixed_frame_, base_frame_, t, trans); gm::Pose::Ptr p(new gm::Pose()); poseTFToMsg(trans, *p); return p; } catch (tf::TransformException& e) { ROS_INFO_THROTTLE(1.0, "Skipping due to lack of transform from %s to %s", base_frame_.c_str(), fixed_frame_.c_str()); } } return gm::Pose::ConstPtr(); }
int main(int argc,char** argv) { ros::init(argc,argv,"roboteq_controller"); ros::NodeHandle n; ros::NodeHandle nh("~"); ros::Subscriber s1,s2,s3; std::string joy_topic,cmd_vel_topic,serial_tx_topic,serial_rx_topic,encoder_topic,status_topic; double max_time_diff; int index; int max_missing; nh.param<std::string>("cmd_vel_topic",cmd_vel_topic,"/fmActuators/cmd_vel"); nh.param<std::string>("serial_rx_topic",serial_rx_topic,"/fmCSP/S0_rx"); nh.param<std::string>("serial_tx_topic",serial_tx_topic,"/fmCSP/S0_tx"); nh.param<std::string>("deadman_joy_topic",joy_topic,"/fmHMI/joy"); nh.param<std::string>("encoder_topic",encoder_topic,"/fmSensors/encoder"); nh.param<std::string>("status_topic",status_topic,"/fmActuators/status"); nh.param<double>("max_time_diff",max_time_diff,0.5); nh.param<int>("deadman_joy_button_index",index,3); nh.param<int>("deadman_joy_max_missing",max_missing,30); RoboTeq controller; controller.deadman_button_id = index; controller.max_deadman = max_missing; controller.max_time_diff = ros::Duration(max_time_diff); controller.serial_publisher = nh.advertise<msgs::serial>(serial_tx_topic,10); controller.encoder_publisher = nh.advertise<msgs::encoder>(encoder_topic,10); controller.status_publisher = nh.advertise<msgs::motor_status>(status_topic,10); s1 = nh.subscribe<msgs::serial>(serial_rx_topic,10,&RoboTeq::onSerialMsgReceived,&controller); s2 = nh.subscribe<geometry_msgs::TwistStamped>(cmd_vel_topic,10,&RoboTeq::onCmdVelReceived,&controller); s3 = nh.subscribe<sensor_msgs::Joy>(joy_topic,10,&RoboTeq::onJoy,&controller); ros::Rate r(5); while(controller.serial_publisher.getNumSubscribers() != 0) { ROS_INFO_THROTTLE(1,"Waiting for serial node to subscribe"); r.sleep(); } controller.transmitSelfInit(); r.sleep(); ros::Timer t = nh.createTimer(ros::Duration(0.05),&RoboTeq::spin,&controller); ros::spin(); return 0; }
void ISBTS::tsUpdate() { ts_count_.uLong++; ROS_INFO_THROTTLE(1,"count %d AND as on canbus %d %d %d %d bootcount %d CB %d %d", ts_count_.uLong , ts_count_.byte[0], ts_count_.byte[1], ts_count_.byte[2], ts_count_.byte[3],ts_bootcount_.uShort,ts_bootcount_.byte[0],ts_bootcount_.byte[1]); processCanTxEvent(); }
/// PINHOLE void PreProcNode::incomingImage(const sensor_msgs::ImageConstPtr& msg){ if (pubCamera.getNumSubscribers()>0){ /// Measure HZ, Processing time, Image acquisation time ros::WallTime time_s0 = ros::WallTime::now(); /// Get CV Image cv_bridge::CvImageConstPtr cvPtr; //cv_bridge::CvImagePtr cvPtr; try { //cvPtr = cv_bridge::toCvShare(msg, enc::MONO8); cvPtr = cv_bridge::toCvShare(msg, colors[colorId]); //cvPtr = cv_bridge::toCvCopy(msg, enc::MONO8); //cvPtr = cv_bridge::toCvCopy(msg, enc::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR_STREAM_THROTTLE(1,"Failed to understand incoming image:" << e.what()); return; } /// PreProcess Frame cv::Mat image = preproc.process(cvPtr->image); /// PTAM Rectification cv::Mat imageRect; imageRect = camModel.rectify(image); /* /// ////// Test Bearing Vectors std::vector<cv::Point2f> kps; bool found = cv::findChessboardCorners(imageRect, cv::Size(8,6), kps); //cv::drawChessboardCorners(imageRect, cv::Size(8,6), kps, found); //std::vector<cv::KeyPoint> kps; //cv::FAST(imageRect, kps, 100.5, true); //cv::drawKeypoints(imageRect, kps, imageRect); //cv::drawChessboardCorners(imageRect, cv::Size(8,6), kps, found); Eigen::MatrixXd bv; camModel.bearingVectors(kps, bv); const tf::Quaternion q(1,0,0,0); for (uint i=0; i<kps.size(); ++i){ pubTF.sendTransform(tf::StampedTransform(tf::Transform(q,tf::Vector3(bv(i,0),bv(i,1),bv(i,2))), ros::Time::now(), "/cam", "/F"+boost::lexical_cast<std::string>(i))); } if (imageRect.channels() != image.channels()){ cv::cvtColor(imageRect, imageRect,CV_BGR2GRAY); } /// ////// try{ // sent out by the crazyflie driver driver.py tf::StampedTransform imu; subTF.waitForTransform("/world", "/cf", msg->header.stamp-ros::Duration(0), ros::Duration(0.05) ); subTF.lookupTransform("/world", "/cf", msg->header.stamp-ros::Duration(0), imu); double r,p,y; OVO::tf2RPY(imu, r,p,y); camModel.rotatePoints(kps, r); } catch(tf::TransformException& ex){ ROS_ERROR_THROTTLE(1,"TF exception. Could not get flie IMU transform: %s", ex.what()); } */ sensor_msgs::CameraInfoPtr infoMsgPtr = camModel.getCamInfo(); /// Send out cv_bridge::CvImage cvi; cvi.header.stamp = msg->header.stamp; cvi.header.seq = msg->header.seq; cvi.header.frame_id = msg->header.frame_id; //cvi.encoding = enc::MONO8; cvi.encoding = colors[colorId]; cvi.image = imageRect; infoMsgPtr->header = cvi.header; pubCamera.publish(cvi.toImageMsg(), infoMsgPtr); // Compute running average of processing time timeAvg = timeAvg*timeAlpha + (1.0 - timeAlpha)*(ros::WallTime::now()-time_s0).toSec(); ROS_INFO_THROTTLE(1, "Processing Time: %.1fms", timeAvg*1000.); } else { //ROS_INFO_THROTTLE(5, "Not processing images as not being listened to"); } }
/**This function reads the sensor input from a bagfile specified in the parameter bagfile_name. * It is meant for offline processing of each frame */ void BagLoader::loadBag(std::string filename) { ScopedTimer s(__FUNCTION__); ros::NodeHandle nh; ParameterServer* ps = ParameterServer::instance(); std::string points_tpc = ps->get<std::string>("individual_cloud_out_topic").c_str();//ps->get<std::string>("topic_points"); ROS_INFO_STREAM("Listening to " << points_tpc); std::string tf_tpc = std::string("/tf"); tf_pub_ = nh.advertise<tf::tfMessage>(tf_tpc, 10); ROS_INFO("Loading Bagfile %s", filename.c_str()); Q_EMIT setGUIStatus(QString("Loading ")+filename.c_str()); { //bag will be destructed after this block (hopefully frees memory for the optimizer) rosbag::Bag bag; try{ bag.open(filename, rosbag::bagmode::Read); } catch(rosbag::BagIOException ex) { ROS_FATAL("Opening Bagfile %s failed: %s Quitting!", filename.c_str(), ex.what()); ros::shutdown(); return; } ROS_INFO("Opened Bagfile %s", filename.c_str()); Q_EMIT setGUIStatus(QString("Opened ")+filename.c_str()); // Image topics to load for bagfiles std::vector<std::string> topics; topics.push_back(points_tpc); topics.push_back(tf_tpc); rosbag::View view(bag, rosbag::TopicQuery(topics)); // Simulate sending of the messages in the bagfile std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds; ros::Time last_tf(0); BOOST_FOREACH(rosbag::MessageInstance const m, view) { if(!ros::ok()) return; while(pause_) { usleep(100); ROS_INFO_THROTTLE(5.0,"Paused - press Space to unpause"); if(!ros::ok()) return; } ROS_INFO_STREAM("Processing message on topic " << m.getTopic()); if (m.getTopic() == points_tpc || ("/" + m.getTopic() == points_tpc)) { sensor_msgs::PointCloud2::ConstPtr pointcloud = m.instantiate<sensor_msgs::PointCloud2>(); //if (cam_info) cam_info_sub_->newMessage(cam_info); if (pointcloud) pointclouds.push_back(pointcloud); ROS_INFO("Found Message of %s", points_tpc.c_str()); } if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){ tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>(); if (tf_msg) { //if(tf_msg->transforms[0].header.frame_id == "/kinect") continue;//avoid destroying tf tree if odom is used //prevents missing callerid warning boost::shared_ptr<std::map<std::string, std::string> > msg_header_map = tf_msg->__connection_header; (*msg_header_map)["callerid"] = "rgbdslam"; tf_pub_.publish(tf_msg); ROS_INFO("Found Message of %s", tf_tpc.c_str()); last_tf = tf_msg->transforms[0].header.stamp; last_tf -= ros::Duration(0.3); } } while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){ Q_EMIT setGUIInfo(QString("Processing frame ") + QString::number(data_id_)); callback(pointclouds.front()); pointclouds.pop_front(); } } bag.close(); } ROS_INFO("Bagfile fully processed"); Q_EMIT setGUIStatus(QString("Done with ")+filename.c_str()); Q_EMIT bagFinished(); }
FootstepMarker::FootstepMarker(): ac_("footstep_planner", true), ac_exec_("footstep_controller", true), plan_run_(false), lleg_first_(true) { // read parameters tf_listener_.reset(new tf::TransformListener); ros::NodeHandle pnh("~"); ros::NodeHandle nh; srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh); typename dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind (&FootstepMarker::configCallback, this, _1, _2); srv_->setCallback (f); pnh.param("foot_size_x", foot_size_x_, 0.247); pnh.param("foot_size_y", foot_size_y_, 0.135); pnh.param("foot_size_z", foot_size_z_, 0.01); pnh.param("lfoot_frame_id", lfoot_frame_id_, std::string("lfsensor")); pnh.param("rfoot_frame_id", rfoot_frame_id_, std::string("rfsensor")); pnh.param("show_6dof_control", show_6dof_control_, true); // pnh.param("use_projection_service", use_projection_service_, false); // pnh.param("use_projection_topic", use_projection_topic_, false); pnh.param("always_planning", always_planning_, true); // if (use_projection_topic_) { project_footprint_pub_ = pnh.advertise<jsk_interactive_marker::SnapFootPrintInput>("project_footprint", 1); // } // read lfoot_offset readPoseParam(pnh, "lfoot_offset", lleg_offset_); readPoseParam(pnh, "rfoot_offset", rleg_offset_); pnh.param("footstep_margin", footstep_margin_, 0.2); pnh.param("use_footstep_planner", use_footstep_planner_, true); pnh.param("use_footstep_controller", use_footstep_controller_, true); pnh.param("use_initial_footstep_tf", use_initial_footstep_tf_, true); pnh.param("wait_snapit_server", wait_snapit_server_, false); bool nowait = true; pnh.param("no_wait", nowait, true); pnh.param("frame_id", marker_frame_id_, std::string("/map")); footstep_pub_ = nh.advertise<jsk_footstep_msgs::FootstepArray>("footstep_from_marker", 1); snapit_client_ = nh.serviceClient<jsk_pcl_ros::CallSnapIt>("snapit"); snapped_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("snapped_pose", 1); current_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("current_pose", 1); estimate_occlusion_client_ = nh.serviceClient<std_srvs::Empty>("require_estimation"); if (!nowait && wait_snapit_server_) { snapit_client_.waitForExistence(); } if (pnh.getParam("initial_reference_frame", initial_reference_frame_)) { use_initial_reference_ = true; JSK_ROS_INFO_STREAM("initial_reference_frame: " << initial_reference_frame_); } else { use_initial_reference_ = false; JSK_ROS_INFO("initial_reference_frame is not specified "); } server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName())); // menu_handler_.insert( "Snap Legs", // boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); // menu_handler_.insert( "Reset Legs", // boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert( "Look Ground", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert( "Execute the Plan", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert( "Force to replan", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); // menu_handler_.insert( "Estimate occlusion", // boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert( "Cancel Walk", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert( "Toggle 6dof marker", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); // menu_handler_.insert( "Resume Footstep", // boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert("Straight Heuristic", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert("Stepcost Heuristic**", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert("LLeg First", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); menu_handler_.insert("RLeg First", boost::bind(&FootstepMarker::menuFeedbackCB, this, _1)); marker_pose_.header.frame_id = marker_frame_id_; marker_pose_.header.stamp = ros::Time::now(); marker_pose_.pose.orientation.w = 1.0; resetLegPoses(); // initialize lleg_initial_pose, rleg_initial_pose lleg_initial_pose_.position.y = footstep_margin_ / 2.0; lleg_initial_pose_.orientation.w = 1.0; rleg_initial_pose_.position.y = - footstep_margin_ / 2.0; rleg_initial_pose_.orientation.w = 1.0; if (use_initial_reference_) { while (ros::ok()) { try { if (!tf_listener_->waitForTransform(marker_frame_id_, initial_reference_frame_, ros::Time(0.0), ros::Duration(10.0))) { ROS_INFO_THROTTLE(1.0, "waiting for transform %s => %s", marker_frame_id_.c_str(), initial_reference_frame_.c_str()); continue; } ROS_INFO("resolved transform %s => %s", marker_frame_id_.c_str(), initial_reference_frame_.c_str()); tf::StampedTransform transform; tf_listener_->lookupTransform(marker_frame_id_, initial_reference_frame_, ros::Time(0), transform); marker_pose_.pose.position.x = transform.getOrigin().x(); marker_pose_.pose.position.y = transform.getOrigin().y(); marker_pose_.pose.position.z = transform.getOrigin().z(); marker_pose_.pose.orientation.x = transform.getRotation().x(); marker_pose_.pose.orientation.y = transform.getRotation().y(); marker_pose_.pose.orientation.z = transform.getRotation().z(); marker_pose_.pose.orientation.w = transform.getRotation().w(); break; } catch (tf2::TransformException& e) { ROS_ERROR("Failed to lookup transformation: %s", e.what()); } } } initializeInteractiveMarker(); if (use_footstep_planner_) { ROS_INFO("waiting planner server..."); ac_.waitForServer(); ROS_INFO("found planner server..."); } if (use_footstep_controller_) { ROS_INFO("waiting controller server..."); ac_exec_.waitForServer(); ROS_INFO("found controller server..."); } move_marker_sub_ = nh.subscribe("move_marker", 1, &FootstepMarker::moveMarkerCB, this); menu_command_sub_ = nh.subscribe("menu_command", 1, &FootstepMarker::menuCommandCB, this); exec_sub_ = pnh.subscribe("execute", 1, &FootstepMarker::executeCB, this); resume_sub_ = pnh.subscribe("resume", 1, &FootstepMarker::resumeCB, this); plan_if_possible_srv_ = pnh.advertiseService("force_to_replan", &FootstepMarker::forceToReplan, this); if (use_initial_footstep_tf_) { // waiting TF while (ros::ok()) { try { if (tf_listener_->waitForTransform(lfoot_frame_id_, marker_frame_id_, ros::Time(0.0), ros::Duration(10.0)) && tf_listener_->waitForTransform(rfoot_frame_id_, marker_frame_id_, ros::Time(0.0), ros::Duration(10.0))) { break; } ROS_INFO("waiting for transform {%s, %s} => %s", lfoot_frame_id_.c_str(), rfoot_frame_id_.c_str(), marker_frame_id_.c_str()); } catch (tf2::TransformException& e) { ROS_ERROR("Failed to lookup transformation: %s", e.what()); } } ROS_INFO("resolved transform {%s, %s} => %s", lfoot_frame_id_.c_str(), rfoot_frame_id_.c_str(), marker_frame_id_.c_str()); } // if (use_projection_topic_) { projection_sub_ = pnh.subscribe("projected_pose", 1, &FootstepMarker::projectionCallback, this); // } }
bool CameraProjections::Update() { try { diagnalAngleView = params.camera.diagonalAngleView->get(); if (dummy && (ros::Time::now() < lastTime)) { ROS_WARN( "Vision Module detected a backward time. Cleared all cached tf data."); m_tf->clear(); delete m_tf; m_tf = new tf::TransformListener(ros::Duration(10)); m_tf->waitForTransform("/ego_rot", "/camera_optical", ros::Time::now() - ros::Duration(params.debug.timeToShift->get()), ros::Duration(1.0)); } lastTime = ros::Time::now(); topViewMarker.clear(); ros::Time past = ros::Time::now() - ros::Duration(0.060); //To get the 40ms before if (dummy) { past = ros::Time(0); //To get latest } m_tf->lookupTransform("/ego_rot", "/camera_optical", ros::Time::now() - ros::Duration(params.debug.timeToShift->get()), tfOptical); tfScalar roll, pitch, yaw; tfOptical.getBasis().getEulerYPR(yaw, pitch, roll); // cout<< " X = "<<tfOptical.getOrigin().getX() << " Y= "<< tfOptical.getOrigin().getY()<< " Z="<< tfOptical.getOrigin().getZ()<<endl; OpticalAngle.x = pitch; OpticalAngle.y = -(roll + M_PI); OpticalAngle.z = yaw + M_PI / 2.; if (VERBOSE) { ROS_INFO_THROTTLE(0.33, "x= %1.2f y= %1.2f z=%1.2f", Radian2Degree(OpticalAngle.x), Radian2Degree(OpticalAngle.y), Radian2Degree(OpticalAngle.z)); } cameraLocation.x = params.location.x->get(); //todo: it seems that the actual height published in odometry cameraLocation.y = params.location.y->get(); cameraLocation.z = params.location.z->get(); cameraOrintation.x = OpticalAngle.x + params.orientation.x->get(); cameraOrintation.y = OpticalAngle.y + params.orientation.y->get(); cameraOrintation.z = CorrectAngleRadian360( OpticalAngle.z + params.orientation.z->get()); } catch (tf::TransformException &ex) { std::string errorMsg(ex.what()); // if(errorMsg.find("Lookup would require extrapolation")!=std::string::npos) // { // reset_time_pub.publish(std_msgs::Empty()); // reset_clock_pub.publish(std_msgs::Empty()); // } ROS_ERROR("%s", errorMsg.c_str()); return false; } return true; }
/*********************************************************************** * Method: RobotController::Init * Params: ros::NodeHandle *nh, int robotID, std::string robotName, int storage_cap, int storage_used, bool type * Returns: void * Effects: Initialize the controller with parameters ***********************************************************************/ void RobotController::Init(ros::NodeHandle *nh, int robotID, std::string robotName, int storage_cap, int storage_used, RobotState::Type type) { m_nh = nh; m_listener.reset( new tf::TransformListener(*nh) ); if (m_nh->getParam("controller/robot_id", robotID)) ROS_INFO_STREAM("Set robot ID: " << robotID); else ROS_ERROR_STREAM("Did not read robot ID: default = " << robotID); m_status.SetID(robotID); if (m_nh->getParam("controller/robot_name", robotName)) ROS_INFO_STREAM("Read robot name: " << robotName); else ROS_ERROR_STREAM("Did not read robot name: default = " << robotName); m_status.SetName(robotName); if(m_nh->getParam("controller/storage_capacity", storage_cap)) ROS_INFO_STREAM("Read storage capacity: " << storage_cap); else ROS_ERROR_STREAM("Did not read storage capacity: default = " << storage_cap); m_status.SetStorageCapacity(storage_cap); if(m_nh->getParam("controller/storage_used", storage_used)) ROS_INFO_STREAM("Read storage used: " << storage_used); else ROS_ERROR_STREAM("Did not read storage used: default = " << storage_used); m_status.SetStorageUsed(storage_used); std::string robotType; if (m_nh->getParam("controller/type", robotType)) ROS_INFO_STREAM("Read robot type: " << robotType); else ROS_ERROR_STREAM("Did not read type: default = " << robotType); if (robotType.compare("collector") == 0) { ROS_INFO_STREAM("Type: Collector"); type = RobotState::COLLECTOR_BOT; } else { ROS_INFO_STREAM("Type: Binbot"); type = RobotState::BIN_BOT; } m_status.SetType(type); std::string tf_prefix; if (m_nh->getParam("controller/tf_prefix", tf_prefix)) ROS_INFO_STREAM("Read tf prefix: " << tf_prefix); else ROS_ERROR_STREAM("Did not read tf_prefix: default = " << tf_prefix); base_frame = std::string("base_link"); if (m_nh->getParam("controller/base_frame", base_frame)) ROS_INFO_STREAM("Read base frame: " << base_frame); else ROS_ERROR_STREAM("Did not read base_frame: default = " << base_frame); base_frame = tf_prefix + "/"+base_frame; if (robotID < 0) { ROS_ERROR_STREAM("ERROR: Received invalid robot id: " << robotID); return; } if (storage_cap < 0) { ROS_ERROR_STREAM("ERROR: invalid storage capacity: " << storage_cap); return; } if (storage_used > storage_cap) { ROS_ERROR_STREAM("ERROR: storage used > storage capacity: used=" << storage_used << ", cap=" << storage_cap); return; } // Todo: Only start if fake trash is not used m_tagProcessor.reset( new AprilTagProcessor() ); m_tagProcessor->Init(nh, robotID); action_client_ptr.reset( new MoveBaseClient("move_base", true) ); // Wait for the action server to come up while(ros::ok() && !action_client_ptr->waitForServer(ros::Duration(2.0))){ ROS_INFO_THROTTLE(3.0,"Waiting for the move_base action server to come up"); } SetupCallbacks(); ROS_DEBUG_STREAM("Robot has setup the movebase client"); Transition(RobotState::WAITING); m_timeEnteringState = ros::Time::now(); ROS_INFO_STREAM("Finished initializing"); }
int main(int argc, char** argv) { ros::init(argc, argv, "exploration"); ros::NodeHandle nh; ros::Publisher trajectory_pub = nh.advertise < trajectory_msgs::MultiDOFJointTrajectory > (mav_msgs::default_topics::COMMAND_TRAJECTORY, 5); ROS_INFO("Started exploration"); std_srvs::Empty srv; bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); unsigned int i = 0; // Trying to unpause Gazebo for 10 seconds. while (i <= 10 && !unpaused) { ROS_INFO("Wait for 1 second before trying to unpause Gazebo again."); std::this_thread::sleep_for(std::chrono::seconds(1)); unpaused = ros::service::call("/gazebo/unpause_physics", srv); ++i; } if (!unpaused) { ROS_FATAL("Could not wake up Gazebo."); return -1; } else { ROS_INFO("Unpaused the Gazebo simulation."); } double dt = 1.0; std::string ns = ros::this_node::getName(); if (!ros::param::get(ns + "/nbvp/dt", dt)) { ROS_FATAL("Could not start exploration. Parameter missing! Looking for %s", (ns + "/nbvp/dt").c_str()); return -1; } static int n_seq = 0; trajectory_msgs::MultiDOFJointTrajectory samples_array; mav_msgs::EigenTrajectoryPoint trajectory_point; trajectory_msgs::MultiDOFJointTrajectoryPoint trajectory_point_msg; // Wait for 5 seconds to let the Gazebo GUI show up. ros::Duration(5.0).sleep(); // This is the initialization motion, necessary that the known free space allows the planning // of initial paths. ROS_INFO("Starting the planner: Performing initialization motion"); for (double i = 0; i <= 1.0; i = i + 0.1) { nh.param<double>("wp_x", trajectory_point.position_W.x(), 0.0); nh.param<double>("wp_y", trajectory_point.position_W.y(), 0.0); nh.param<double>("wp_z", trajectory_point.position_W.z(), 1.0); samples_array.header.seq = n_seq; samples_array.header.stamp = ros::Time::now(); samples_array.points.clear(); n_seq++; tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), -M_PI * i); trajectory_point.setFromYaw(tf::getYaw(quat)); mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg); samples_array.points.push_back(trajectory_point_msg); trajectory_pub.publish(samples_array); ros::Duration(1.0).sleep(); } trajectory_point.position_W.x() -= 0.5; trajectory_point.position_W.y() -= 0.5; samples_array.header.seq = n_seq; samples_array.header.stamp = ros::Time::now(); samples_array.points.clear(); n_seq++; mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg); samples_array.points.push_back(trajectory_point_msg); trajectory_pub.publish(samples_array); ros::Duration(1.0).sleep(); // Start planning: The planner is called and the computed path sent to the controller. int iteration = 0; while (ros::ok()) { ROS_INFO_THROTTLE(0.5, "Planning iteration %i", iteration); nbvplanner::nbvp_srv planSrv; planSrv.request.header.stamp = ros::Time::now(); planSrv.request.header.seq = iteration; planSrv.request.header.frame_id = "world"; if (ros::service::call("nbvplanner", planSrv)) { n_seq++; if (planSrv.response.path.size() == 0) { ros::Duration(1.0).sleep(); } for (int i = 0; i < planSrv.response.path.size(); i++) { samples_array.header.seq = n_seq; samples_array.header.stamp = ros::Time::now(); samples_array.header.frame_id = "world"; samples_array.points.clear(); tf::Pose pose; tf::poseMsgToTF(planSrv.response.path[i], pose); double yaw = tf::getYaw(pose.getRotation()); trajectory_point.position_W.x() = planSrv.response.path[i].position.x; trajectory_point.position_W.y() = planSrv.response.path[i].position.y; // Add offset to account for constant tracking error of controller trajectory_point.position_W.z() = planSrv.response.path[i].position.z + 0.25; tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), yaw); trajectory_point.setFromYaw(tf::getYaw(quat)); mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg); samples_array.points.push_back(trajectory_point_msg); trajectory_pub.publish(samples_array); ros::Duration(dt).sleep(); } } else { ROS_WARN_THROTTLE(1, "Planner not reachable"); ros::Duration(1.0).sleep(); } iteration++; } }
int main(int argc, char **argv) { ros::init(argc, argv, "updating_particles"); ros::NodeHandle n; PlotRayUtils plt; RayTracer rayt; std::random_device rd; std::normal_distribution<double> randn(0.0,0.000001); ROS_INFO("Running..."); ros::Publisher pub_init = n.advertise<particle_filter::PFilterInit>("/particle_filter_init", 5); ros::ServiceClient srv_add = n.serviceClient<particle_filter::AddObservation>("/particle_filter_add"); ros::Duration(1).sleep(); // pub_init.publish(getInitialPoints(plt)); geometry_msgs::Point obs; geometry_msgs::Point dir; double radius = 0.00; int i = 0; //for(int i=0; i<20; i++){ while (i < NUM_TOUCHES) { // ros::Duration(1).sleep(); //tf::Point start(0.95,0,-0.15); //tf::Point end(0.95,2,-0.15); tf::Point start, end; // randomSelection(plt, rayt, start, end); fixedSelection(plt, rayt, start, end); Ray measurement(start, end); double distToPart; if(!rayt.traceRay(measurement, distToPart)){ ROS_INFO("NO INTERSECTION, Skipping"); continue; } tf::Point intersection(start.getX(), start.getY(), start.getZ()); intersection = intersection + (end-start).normalize() * (distToPart - radius); std::cout << "Intersection at: " << intersection.getX() << " " << intersection.getY() << " " << intersection.getZ() << std::endl; tf::Point ray_dir(end.x()-start.x(),end.y()-start.y(),end.z()-start.z()); ray_dir = ray_dir.normalize(); obs.x=intersection.getX() + randn(rd); obs.y=intersection.getY() + randn(rd); obs.z=intersection.getZ() + randn(rd); dir.x=ray_dir.x(); dir.y=ray_dir.y(); dir.z=ray_dir.z(); // obs.x=intersection.getX(); // obs.y=intersection.getY(); // obs.z=intersection.getZ(); // pub_add.publish(obs); // plt.plotCylinder(start, end, 0.01, 0.002, true); plt.plotRay(Ray(start, end)); // ros::Duration(1).sleep(); particle_filter::AddObservation pfilter_obs; pfilter_obs.request.p = obs; pfilter_obs.request.dir = dir; if(!srv_add.call(pfilter_obs)){ ROS_INFO("Failed to call add observation"); } ros::spinOnce(); while(!rayt.particleHandler.newParticles){ ROS_INFO_THROTTLE(10, "Waiting for new particles..."); ros::spinOnce(); ros::Duration(.1).sleep(); } i ++; } // std::ofstream myfile; // myfile.open("/home/shiyuan/Documents/ros_marsarm/diff.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); // myfile.open("/home/shiyuan/Documents/ros_marsarm/time.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); // myfile.open("/home/shiyuan/Documents/ros_marsarm/diff_trans.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); // myfile.open("/home/shiyuan/Documents/ros_marsarm/diff_rot.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); // myfile.open("/home/shiyuan/Documents/ros_marsarm/workspace_max.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); // myfile.open("/home/shiyuan/Documents/ros_marsarm/workspace_min.csv", std::ios::out|std::ios::app); // myfile << "\n"; // myfile.close(); ROS_INFO("Finished all action"); }
bool TreeKinematics::getPositionIk(tree_kinematics::get_tree_position_ik::Request &request, tree_kinematics::get_tree_position_ik::Response &response) { ik_srv_duration_ = ros::Time::now().toSec(); ROS_DEBUG("getPositionIK method invoked."); // extract current joint positions from the request KDL::JntArray q, q_desi; double nr_of_jnts = request.pos_ik_request[0].ik_seed_state.joint_state.name.size(); q.resize(nr_of_jnts); q_desi.resize(nr_of_jnts); for (unsigned int i=0; i < nr_of_jnts; ++i) { int tmp_index = getJointIndex(request.pos_ik_request[0].ik_seed_state.joint_state.name[i]); if (tmp_index >=0) { q(tmp_index) = request.pos_ik_request[0].ik_seed_state.joint_state.position[i]; ROS_DEBUG("joint '%s' is now number '%d'", request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str(), tmp_index); } else { ROS_FATAL("i: %d, No joint index for %s!",i,request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str()); ROS_FATAL("Service call aborted."); return false; } } // convert pose messages into transforms from the root frame to the given poses and further into KDL::Frames geometry_msgs::PoseStamped pose_msg_in; geometry_msgs::PoseStamped pose_msg_transformed; tf::Stamped<tf::Pose> transform; tf::Stamped<tf::Pose> transform_root; KDL::Frames desired_poses; KDL::Frame desired_pose; for(unsigned int i = 0; i < request.pos_ik_request.size(); ++i) { pose_msg_in = request.pos_ik_request[i].pose_stamped; try { tf_listener_.waitForTransform(tree_root_name_, pose_msg_in.header.frame_id, pose_msg_in.header.stamp, ros::Duration(0.1)); tf_listener_.transformPose(tree_root_name_, pose_msg_in, pose_msg_transformed); } catch (tf::TransformException const &ex) { ROS_FATAL("%s",ex.what()); ROS_FATAL("Could not transform IK pose from '%s' to frame '%s'", tree_root_name_.c_str(), pose_msg_in.header.frame_id.c_str()); response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return false; } tf::PoseMsgToKDL(pose_msg_transformed.pose, desired_pose); desired_poses[request.pos_ik_request[i].ik_link_name] = desired_pose; } // use the solver to compute desired joint positions ik_duration_ = ros::Time::now().toSec(); int ik_ret = ik_pos_solver_->CartToJnt_it(q, desired_poses, q_desi); // NOTE: Before it was CartToJnt (without the _it). What's the difference? ik_duration_ = ros::Time::now().toSec() - ik_duration_; ik_duration_median_ = ((ik_duration_median_ * (loop_count_ - 1)) + ik_duration_) / loop_count_; // insert the solver's result into the service response if (ik_ret >= 0 || ik_ret == -3) { response.solution.joint_state.name = info_.joint_names; response.solution.joint_state.position.resize(nr_of_jnts_); response.solution.joint_state.velocity.resize(nr_of_jnts_); for (unsigned int i=0; i < nr_of_jnts_; ++i) { response.solution.joint_state.position[i] = q_desi(i); response.solution.joint_state.velocity[i] = (q_desi(i) - q(i)) * srv_call_frequency_; ROS_DEBUG("IK Solution: %s %d: pos = %f, vel = %f",response.solution.joint_state.name[i].c_str(), i, response.solution.joint_state.position[i], response.solution.joint_state.velocity[i]); } response.error_code.val = response.error_code.SUCCESS; ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_; ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_; loop_count_++; ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_, ik_srv_duration_median_); ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_); if (ik_ret == -3) { ROS_WARN_THROTTLE(1.0, "Maximum iterations reached! (error code = %d)", ik_ret); } } else { ROS_WARN("An IK solution could not be found (error code = %d)", ik_ret); response.error_code.val = response.error_code.NO_IK_SOLUTION; ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_; ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_; loop_count_++; ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_, ik_srv_duration_median_); ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_); } return true; }
/** * \brief Fills data using XsDataPacket object * @param _packet Incoming packet from Xsens device. */ void SensorData::fillData(XsDataPacket * _packet){ XsDataPacket packet = *_packet; XsMessage msg = packet.toMessage(); XsSize msg_size = msg.getDataSize(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Data Size: " << msg_size); // Get the orientation data if (packet.containsOrientation()) { XsQuaternion quaternion = packet.orientationQuaternion(); q1 = quaternion.m_x; q2 = quaternion.m_y; q3 = quaternion.m_z; q0 = quaternion.m_w; XsEuler euler = packet.orientationEuler(); eroll = euler.m_roll; epitch = euler.m_pitch; eyaw = euler.m_yaw; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,"Orientation: Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll << ", Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch << ", Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw); } // Get the gyroscope data if (packet.containsCalibratedGyroscopeData()) { XsVector gyroscope = packet.calibratedGyroscopeData(); gyrX = gyroscope.at(0); gyrY = gyroscope.at(1); gyrZ = gyroscope.at(2); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Angular Velocity: ( " << gyrX << ", " << gyrY << ", " << gyrZ << ")" ); } // Get the acceleration data if (packet.containsCalibratedAcceleration()) { XsVector acceleration = packet.calibratedAcceleration(); accX = acceleration.at(0); accY = acceleration.at(1); accZ = acceleration.at(2); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Linear Acceleration: (" << accX << "," << accX << "," << accZ << ")" ); } //Get the altitude data if(packet.containsAltitude()){ mAltitude = packet.altitude(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsAltitude: "); } //Get the Latitude and Longitude Data if(packet.containsLatitudeLongitude()){ XsVector latlon = packet.latitudeLongitude(); mLatitude = latlon[0]; mLongitude = latlon[1]; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsLatitudeLongitude"); } // Get the magnetic field data if (packet.containsCalibratedMagneticField()) { XsVector magneticField = packet.calibratedMagneticField(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Magnetic Field: ("<< magneticField[0] <<" , " << magneticField[1] << " , "<< magneticField[2] << ")"); magX = magneticField.at(0); magY = magneticField.at(1); magZ = magneticField.at(2); } // Get Temperature data if (packet.containsTemperature()){ double temperature = packet.temperature(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Temperature : " << temperature); mTemperature = temperature; } // Get Pressure Data if(packet.containsPressure() ){ XsPressure pressure = packet.pressure(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pressure : " << pressure.m_pressure); mPressure= pressure.m_pressure; } // Get Velocity Data if( packet.containsVelocity() ){ XsVector velocity = packet.velocity(); mVelocityX = velocity.at(0); mVelocityY = velocity.at(1); mVelocityZ = velocity.at(2); ROS_INFO_STREAM(" Velocity [ x (east) : " << mVelocityX << ", y (north) : " << mVelocityY << ", z (up) " << mVelocityZ << " ]"); } // Get GPS PVT Data if( packet.containsGpsPvtData() ){ XsGpsPvtData gpsPvtData = packet.gpsPvtData(); ROS_INFO_STREAM(" Horizontal accuracy: " << gpsPvtData.m_hacc ); } if(packet.containsRawAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Acceleration") ;} if(packet.containsRawGyroscopeData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Gyroscope") ;} if(packet.containsRawGyroscopeTemperatureData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawGyrTemp");} if(packet.containsRawMagneticField() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawMag");} if(packet.containsRawTemperature() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawTemp");} if(packet.containsRawData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawData");} if(packet.containsCalibratedData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Calib Data");} if(packet.containsSdiData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SDI");} //if(packet.containsStatusByte() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "StatusByte");} if(packet.containsDetailedStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "DetailedStatus");} if(packet.containsPacketCounter8() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter8");} if(packet.containsPacketCounter() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter");} if(packet.containsSampleTimeFine() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeFine");} if(packet.containsSampleTimeCoarse() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeCoarse");} if(packet.containsSampleTime64() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTime64");} if(packet.containsFreeAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "FreeAcceleration");} if(packet.containsPressureAge() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PressureAge");} if(packet.containsAnalogIn1Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN1IN");} if(packet.containsAnalogIn2Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN2IN");} if(packet.containsPositionLLA() ){ XsVector posLLA = packet.positionLLA(); mLatitude = posLLA[0]; mLongitude = posLLA[1]; mAltitude = posLLA[2]; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLatitude = " << mLatitude); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLongitude = " << mLongitude); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mAltitude = " << mAltitude); //for(int i=0; i < posLLA.size(); i++){ //ROS_INFO_STREAM("posLLA[" << i << "] : " << posLLA[i] ); //} } if(packet.containsUtcTime() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "UTC-Time");} if(packet.containsFrameRange() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Frame Range");} if(packet.containsRssi() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RSSI");} if(packet.containsRawGpsDop() ){ //ROS_INFO_THROTTLE(THROTTLE_VALUE, "DOP");} XsRawGpsDop dop = packet.rawGpsDop(); m_gdop = ((float) dop.m_gdop)/100; m_pdop = ((float) dop.m_pdop)/100; m_tdop = ((float) dop.m_tdop)/100; m_vdop = ((float) dop.m_vdop)/100; m_hdop = ((float) dop.m_hdop)/100; m_edop = ((float) dop.m_edop)/100; m_ndop = ((float) dop.m_ndop)/100; m_itow = ((float) dop.m_itow)/1000; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "GDOP = " << m_gdop << ",PDOP = " << m_pdop << ",TDOP = " << m_tdop << ",VDOP = " << m_vdop << ",HDOP = " << m_hdop << ",EDOP = " << m_edop << ",NDOP = " << m_ndop << ",ITOW = " << m_itow); } if(packet.containsRawGpsSol() ){ // ROS_INFO_THROTTLE(THROTTLE_VALUE, "SOL");} XsRawGpsSol sol = packet.rawGpsSol(); mPositionAccuracy = sol.m_pacc; mSpeedAccuracy = sol.m_sacc; mSatelliteNumber = sol.m_numsv; mGpsFixStatus = sol.m_gpsfix; gpsVelocityX=sol.m_ecef_vx/100; gpsVelocityY=sol.m_ecef_vy/100; gpsVelocityZ=sol.m_ecef_vz/100; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pos Acc = " << mPositionAccuracy << ",Speed Acc = " << mSpeedAccuracy << ",Sat Number = " << mSatelliteNumber << ",GPS FIX = " << std::hex << mGpsFixStatus); } if(packet.containsRawGpsTimeUtc() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "GPS-UTC");} if(packet.containsRawGpsSvInfo() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SV INFO");} // if(packet.containsTriggerIndication() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "TRIGGER");} //Get Status byte if( packet.containsStatus() ){ mStatus = packet.status(); ROS_INFO_THROTTLE(10, "Status: %.8X", mStatus); } }
void mavrosPoseController::control_loop(){ ros::Rate r( rate_ ); while( alive_.try_lock() && ros::ok() ) { feedback_data_lock_.lock(); std::shared_ptr<geometry_msgs::PoseStamped> tmp_pose( cur_pose_ ); std::shared_ptr<geometry_msgs::TwistStamped> tmp_twist( cur_twist_ ); feedback_data_lock_.unlock(); setpoint_data_lock_.lock(); std::shared_ptr<geometry_msgs::Pose> tmp_setpoint( desired_set_point_ ); setpoint_data_lock_.unlock(); // current time double ct = ros::Time::now().toSec(); // process the cascaded z pid loops ROS_INFO_THROTTLE(1,"::::::::::::::::::::::::::::::::::::::::::::"); ROS_INFO_THROTTLE(1,"::::::::::::::::::::[pos]:::::::::::::::::::"); z_pos_->setSetPoint( tmp_setpoint->position.z ); double desired_z_vel = z_pos_->process( ct, tmp_pose->pose.position.z ); ROS_INFO_THROTTLE(1, "[CE]::%f [DE]::%f [SE]::%f", z_pos_->getCE(), z_pos_->getDE(), z_pos_->getSE() ); y_pos_->setSetPoint( tmp_setpoint->position.y ); double desired_y_vel = y_pos_->process( ct, tmp_pose->pose.position.y ); x_pos_->setSetPoint( tmp_setpoint->position.x ); double desired_x_vel = x_pos_->process( ct, tmp_pose->pose.position.x ); ROS_INFO_THROTTLE(1, "[x]::%f [y]::%f [z]::%f", desired_x_vel, desired_y_vel, desired_z_vel ); ROS_INFO_THROTTLE(1, "::::::::::::::::::::[vel]:::::::::::::::::::"); z_vel_->setSetPoint( desired_z_vel ); y_vel_->setSetPoint( desired_y_vel ); x_vel_->setSetPoint( desired_x_vel ); double z_cmd = z_vel_->process( ct, tmp_twist->twist.linear.z ); ROS_INFO_THROTTLE(1, "[desired_z_cmd]::%f", (z_cmd)/z_throttle_divider_ + z_throttle_zero_ ); std_msgs::Float64 throttle; throttle.data = ((z_cmd)/z_throttle_divider_) + z_throttle_zero_; attThrottlePub.publish(throttle); double y_world_cmd = y_vel_->process( ct, tmp_twist->twist.linear.y ); double x_world_cmd = x_vel_->process( ct, tmp_twist->twist.linear.x ); double mag = sqrt(x_world_cmd*x_world_cmd + y_world_cmd*y_world_cmd); double theta = atan2(y_world_cmd, x_world_cmd); ROS_INFO_THROTTLE(1, "[x_world_cmd]::%f [y_world_cmd]::%f [mag]::%f [theta]::%f", x_world_cmd, y_world_cmd, mag, theta); double cr, cp, yaw; tf::Quaternion q = tf::Quaternion( tmp_pose->pose.orientation.x, tmp_pose->pose.orientation.y, tmp_pose->pose.orientation.z, tmp_pose->pose.orientation.w ); tf::Matrix3x3 m = tf::Matrix3x3(q); m.getRPY(cr, cp, yaw); ROS_INFO_THROTTLE(1, "[yaw]::%f", yaw); double pitch_cmd = (sin(yaw)*x_world_cmd + cos(yaw)*y_world_cmd); double roll_cmd = cos(yaw)*x_world_cmd - sin(yaw)*y_world_cmd; ROS_INFO_THROTTLE(1, "[pitch_cmd]::%f [roll_cmd]::%f", (pitch_cmd), roll_cmd ); geometry_msgs::PoseStamped att_twist; att_twist.header.stamp = ros::Time::now(); att_twist.pose.orientation.x = roll_cmd; att_twist.pose.orientation.y = pitch_cmd; att_twist.pose.orientation.w = 1.0; attitudePub.publish(att_twist); // some debugging publishers std_msgs::Float64 roll; roll.data = roll_cmd; attRollPub.publish(roll); std_msgs::Float64 pitch; pitch.data = pitch_cmd; attPitchPub.publish(pitch); alive_.unlock(); r.sleep(); } }
void Channel::onTimer(const ros::TimerEvent& e, RoboTeQ::status_t& status) { /* Register time */ ros::Time now = ros::Time::now(); std::stringstream ss, out; /* streams for holding status message and command output */ double period = 0, feedback = 0, feedback_filtered = 0, current_velocity = feedback_filter.get()*ticks_to_meter; if(status.online) /* is set when controller answers to FID request */ { ss << "controller_online "; if(status.initialised) /* is set when init function completes */ { ss << "controller_initialised "; if(status.responding) /* is set if the controller publishes serial messages */ { ss << "controller_responding "; if(status.cmd_vel_publishing) /* is set if someone publishes twist messages */ { ss << "cmd_vel_publishing "; if(status.deadman_pressed) /* is set if someone publishes true on deadman topic */ { ss << "deadman_pressed "; if(status.emergency_stop) { /* release emergency stop */ transmit("!MG\r"); status.emergency_stop = false; current_setpoint = 0; } /* Calculate period */ period = (now - time_stamp.last_regulation).toSec(); /* Get latest feedback and reset */ feedback = (( (double)hall_value)*ticks_to_meter)/period; hall_value = 0; /* Filter feedback */ feedback_filtered = velocity_filter.update(feedback); /* Get new setpoint from regulator */ //current_setpoint += regulator.output_from_input(velocity, feedback_filtered , period); current_setpoint = velocity + regulator.output_from_input(velocity, current_velocity , period); current_thrust = (int)(current_setpoint * mps_to_thrust); /* Implement maximum output*/ if(current_thrust > max_output) current_thrust = max_output; if(current_thrust < -max_output) current_thrust = -max_output; /* Send motor output command */ out << "!G " << ch << " " << current_thrust << "\r"; transmit(out.str()); // Upkeep time_stamp.last_regulation = now; } else /* deadman not pressed */ { /* Set speeds to 0 and activate emergency stop */ transmit("!EX\r"); status.emergency_stop = true; transmit("!G 1 0\r"); transmit("!G 2 0\r"); current_setpoint = current_thrust = 0; velocity = 0; regulator.reset_integrator(); } } else /* Cmd_vel is not publishing */ { /* Set speeds to 0 and activate emergency stop */ transmit("!EX\r"); status.emergency_stop = true; transmit("!G 1 0\r"); transmit("!G 2 0\r"); current_setpoint = current_thrust = 0; velocity = 0; regulator.reset_integrator(); } } else /* controller is not responding */ { ROS_INFO_THROTTLE(5,"%s: Controller is not responding",ros::this_node::getName().c_str()); down_time++; if(down_time > 10) { /* Try to re-connect and re-initialise */ transmit("?FID\r"); down_time = 0; } } } else /* Controller is not initialised */ { ROS_INFO("%s: Controller is not initialised",ros::this_node::getName().c_str()); //initController("standard"); //status.initialised = true;' transmit("?FID\r"); } } else /* controller is not online */ { ROS_INFO_THROTTLE(5,"%s: Controller is not yet online",ros::this_node::getName().c_str()); /* Try to re-connect and re-initialise */ transmit("?FID\r"); } /* Publish feedback */ message.feedback.header.stamp = now; message.feedback.velocity = current_velocity; /* Velocity in m/s */ message.feedback.velocity_setpoint = velocity; message.feedback.thrust = (current_thrust*100)/roboteq_max; /* Thrust in % */ publisher.feedback.publish(message.feedback); /* Publish the status message */ message.status.header.stamp = ros::Time::now(); message.status.data = ss.str(); publisher.status.publish(message.status); }
void OrchardDetector::processLaserScan( const sensor_msgs::LaserScan::ConstPtr& laser_scan) { float rthetamean = 0, rrhomean = 0, lthetamean = 0, lrhomean = 0, theta = 0, rho = 0; double x0 = 0, y0 = 0, a, b; int lmc = 0, rmc = 0; static int count = 0; clearRawImage(); sensor_msgs::PointCloud cloud; projector_.projectLaser(*laser_scan, cloud); int size = cloud.points.size(); for (int i = 0; i < size; i++) { if (abs(cloud.points[i].y) < 1.5 && abs(cloud.points[i].y) > 0.5 &&cloud.points[i].y < 0 && cloud.points[i].x > -1 && cloud.points[i].x < 3) { point1.x = ((int)(cloud.points[i].x * 50) + 300); point1.y = ((int)(cloud.points[i].y * 50) + 300); point2.x = point1.x - 4; point2.y = point1.y; cvLine(rawData_, point1, point2, CV_RGB(255,255,255), 2, CV_AA, 0); } } cvCvtColor(rawData_, workingImg_, CV_BGR2GRAY); //cvThreshold(workingImg_,workingImg_,) cvThreshold(workingImg_,workingImg_, 100, 255, CV_THRESH_BINARY); CvMemStorage* stor = cvCreateMemStorage(0); CvSeq* cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), stor); // find external contours cvFindContours(workingImg_, stor, &cont, sizeof(CvContour), CV_RETR_EXTERNAL, 2, cvPoint(0, 0)); for (; cont; cont = cont->h_next) { // size of pointArray and polygon int point_cnt = cont->total; // no small contours if (point_cnt > 20) { CvPoint* PointArray = (CvPoint*) malloc(point_cnt * sizeof(CvPoint)); // Get contour point set. cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); for (int i = 0; i <= point_cnt; i++) { // Show the Pixelcoordinates // have some fun with the color cvLine(rawData_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 0, 0), 10); cvLine(workingImg_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 0, 0), 50); } continue; } // Allocate memory for contour point set. CvPoint* PointArray = (CvPoint*) malloc(point_cnt * sizeof(CvPoint)); // Get contour point set. cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); for (int i = 0; i <= point_cnt; i++) { // Show the Pixelcoordinates //cout << PointArray[i].x << " " << PointArray[i].y << endl; // have some fun with the color int h_value = int(i * 3.0 / point_cnt * i) % 100; cvLine(rawData_, PointArray[i % point_cnt], PointArray[(i + 1) % point_cnt], cvScalar(0, 255, 0), 4); } } //cvDilate(workingImg_,workingImg_, 0, 3); //cvErode(workingImg_,workingImg_, 0, 3); //cvShowImage("Wporking", workingImg_); cvWaitKey(10); lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_STANDARD, 1, CV_PI / 180, 15, 0, 0); //cvHoughLines2(edgesImage, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/360, 30, 10, MAXIMUM_GAP); for (int i = 0; i < MIN(lines_->total,15); i++) { float* line = (float*) cvGetSeqElem(lines_, i); rho = line[0]; theta = line[1]; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)); point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)); point2.y = cvRound(y0 - 600 * (a)); point3.x = 300, point3.y = 300; point4.x = 300, point4.y = 600; point5.x = 300, point5.y = 0; cvLine(rawData_, point3, point4, CV_RGB(0,0,255), 1, CV_AA, 0); cvLine(rawData_, point3, point5, CV_RGB(0,0,255), 1, CV_AA, 0); if (intersect(point1, point2, point3, point4)) { { if(abs(left_angle_ -( (theta) - CV_PI / 2)) < 0.2){ rrhomean += rho; rthetamean += theta; rmc++; cvLine(workingImg_, point1, point2, CV_RGB(0,0,255), 1, CV_AA, 0); } } } else if (intersect(point1, point2, point3, point5)) { { if(abs(right_angle_ -( (theta) - CV_PI / 2)) < 0.5){ lrhomean += rho; lthetamean += theta; lmc++; cvLine(workingImg_, point1, point2, CV_RGB(255,255,255), 1,CV_AA, 0); } } } } if(lmc > 5){ theta = lthetamean / lmc; rho = lrhomean / lmc; a = cos(theta); b = sin(theta); x0 = a * rho; y0 = b * rho; point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a)); point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a)); cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 3, CV_AA, 0); point4.x = 300; point4.y = 300; point5.x = point4.x + 200 * sin(CV_PI - (theta - CV_PI / 2)); point5.y = point4.y + 200 * cos(CV_PI - (theta - CV_PI / 2)); cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0); rows_.header.stamp = ros::Time::now(); rows_.leftvalid = false; rows_.rightvalid = false; if (intersect(point1, point2, point4, point5)) { right_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y - 300)) + ((intersection_.x - 300) * (intersection_.x - 300))) * 2; right_angle_ = (theta) - CV_PI / 2; count++; rolling_mean_[count % 100][0] = right_angle_; right_angle_ = 0; for (int i = 0; i < 100; i++) { right_angle_ += rolling_mean_[i][0]; } right_angle_ = right_angle_ / 100; rolling_mean_[count % 50][1] = right_distance_; right_distance_ = 0; for (int i = 0; i < 50; i++) { right_distance_ += rolling_mean_[i][1]; } right_distance_ = right_distance_ / 50; inrow_cnt++; if(inrow_cnt > 10) inrow_cnt = 10; /* ROS_INFO("angle: %f",right_angle_); //cvLine(rawData_, point1, point2, CV_RGB(0,255,0), 1, CV_AA, 0); */ geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw( right_angle_); marker_r.header.frame_id = "/laser_link"; marker_r.header.stamp = ros::Time::now(); marker_r.ns = "basic_shapes"; marker_r.id = 0; // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER marker_r.type = visualization_msgs::Marker::CUBE; // Set the marker action. Options are ADD and DELETE marker_r.action = visualization_msgs::Marker::ADD; // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header marker_r.pose.position.x = 0; marker_r.pose.position.y = -((float) right_distance_) / 100; marker_r.pose.position.z = 0; marker_r.pose.orientation = pose_quat; // Set the scale of the marker -- 1x1x1 here means 1m on a side marker_r.scale.x = 10.0; marker_r.scale.y = 0.1; marker_r.scale.z = 0.5; // Set the color -- be sure to set alpha to something non-zero! marker_r.color.r = 0.0f; marker_r.color.g = 1.0f; marker_r.color.b = 0.0f; marker_r.color.a = 0.5; marker_r.lifetime = ros::Duration(.5); // Publish the marker marker_r_pub.publish(marker_r); rows_.rightvalid = true; rows_.rightdistance = ((float) right_distance_) / 100; rows_.rightangle = right_angle_; } else { inrow_cnt--; inrow_cnt--; if(inrow_cnt < -5){ inrow_cnt = -5; } /* left_distance_ = -1; left_angle_ = 6000; rows_.rightdistance = ((float) left_distance_) / 100; rows_.rightangle = theta - CV_PI / 2; */ // printf("\nDistance from right:%dmm",hough_lines.right_distance); //sprintf(textright, "Distance from right: %dmm, angle: %d",hough_lines.right_distance, hough_lines.right_angle); } }else{ ROS_INFO_THROTTLE(1,"lmc = %d", lmc); inrow_cnt--; inrow_cnt--; if(inrow_cnt < -5){ inrow_cnt = -5; } } CvFont font; cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, CV_AA); if(inrow_cnt > 0){ twist_msg_.twist.angular.z = -((right_angle_*1.5) - (((float)right_distance_/100) - 1.2) ); cvPutText(rawData_, "INROW-NAVIGATION", cvPoint(10, 130), &font, cvScalar(255, 255, 255, 0)); cvLine(rawData_, cvPoint(10, 140), cvPoint(10 + abs(inrow_cnt * 20),140),CV_RGB(0,255,0), 3, CV_AA, 0); rows_.headland = false; }else{ twist_msg_.twist.angular.z = M_PI/4; cvPutText(rawData_, "HEADLAND", cvPoint(10, 130), &font, cvScalar(255, 255, 255, 0)); cvLine(rawData_, cvPoint(10, 140), cvPoint(10 + abs(inrow_cnt * 20), 140 ),CV_RGB(255,0,0), 3, CV_AA, 0); rows_.headland = true; } twist_pub.publish(twist_msg_); cvLine(rawData_, cvPoint(0, 300 + 150), cvPoint(600, 300 + 150),CV_RGB(255,255,255), 1, CV_AA, 0); cvLine(rawData_, cvPoint(0, 300 - 150), cvPoint(600, 300 - 150),CV_RGB(255,255,255), 1, CV_AA, 0); rows_.header.stamp = ros::Time::now(); row_pub.publish(rows_); cvShowImage("TEST", rawData_); cvWaitKey(10); //pc_pub.publish(cloud); }
void handleMessage(const nav_msgs::OdometryConstPtr &msg) { ROS_INFO_THROTTLE(1.0, "Receiving messages."); }