void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel) { if (!command_tested) { ROS_ERROR_ONCE("Test of robot base driver is not completed. Can't move base."); return; }; double lin = cmd_vel->linear.x; double ang = cmd_vel->angular.z; if (lin > max_lin) lin = max_lin; if (lin < -max_lin) lin = -max_lin; if (ang > max_ang) lin = max_ang; if (ang < -max_ang) ang = -max_ang; //double dt = ros::Time::now().toSec() - latest_command.toSec(); g_lin = lin; g_ang = ang; latest_command = ros::Time::now(); }
double WholeBodyController::getJointPosition(const std::string& joint_name) const { std::map<std::string,unsigned int>::const_iterator index_iter = joint_name_to_index_.find(joint_name); if (index_iter != joint_name_to_index_.end()) { return q_current_(index_iter->second); } else { ROS_ERROR_ONCE("Joint %s is not listed",joint_name.c_str()); return 0; } }
/////////////////////////////////////////////////////////////////////////////////////////// //Function: makes publisher and subscriber and read the params for this node. //pre: - //post: /////////////////////////////////////////////////////////////////////////////////////////// Subscriber::Subscriber(){ // Read joy topic sub_VelSub = nh_.subscribe<sensor_msgs::Joy>(SUB_TOPIC_NAME, SUB_TOPIC_BUFFER_SIZE, &Subscriber::joyCallback, this); // Publish Twist message with motorspeed pub_VelPub = nh_.advertise<geometry_msgs::Twist>(PUB_TOPIC_NAME, PUB_TOPIC_BUFFER_SIZE); //define values for safety dScaleAxisX = 0; dScaleAxisY = 0; dScaleAxisZ = 0; //Check params x-axis, y-axis, z-axis if (nh_.hasParam("iAxisX"),nh_.hasParam("iAxisY"), nh_.hasParam("iAxisZ")) { nh_.getParam("iAxisX",iAxisX); nh_.getParam("iAxisY",iAxisY); nh_.getParam("iAxisZ",iAxisZ); ROS_INFO("The motorparameters are initialized with values: Motor1: %i, Motor2: %i, Motor3: %i.", iAxisX, iAxisY, iAxisZ); } else { ROS_ERROR_ONCE("The motorparameter(s) cannot be initialized"); iAxisX=0; iAxisY=0, iAxisZ=0; } //Check params scaler x, y, z if (nh_.hasParam("dScaleAxisX"),nh_.hasParam("dScaleAxisY"),nh_.hasParam("dScaleAxisZ")) { nh_.getParam("dScaleAxisX",dScaleAxisX); nh_.getParam("dScaleAxisY",dScaleAxisY); nh_.getParam("dScaleAxisZ",dScaleAxisZ); ROS_INFO("The scalerparameters are initialized with values: Scaler1: %f, Scaler2: %f, Scaler3: %f.", dScaleAxisX, dScaleAxisY, dScaleAxisZ); } else { ROS_ERROR_ONCE("The scalerparameter(s) cannot be initialized"); dScaleAxisX=0; dScaleAxisY=0, dScaleAxisZ=0; } }
void WholeBodyController::setMeasuredJointPosition(const std::string& joint_name, double pos) { std::map<std::string,unsigned int>::iterator index_iter = joint_name_to_index_.find(joint_name); if (index_iter != joint_name_to_index_.end()) { unsigned int index = index_iter->second; q_current_(index) = pos; //std::cout << joint_name <<"\t"<<q_current_(index) <<"\t"<<index<< std::endl; robot_state_.tree_.rearrangeJntArrayToTree(q_current_); //robot_state_.collectFKSolutions(); } else { ROS_ERROR_ONCE("Joint %s is not listed",joint_name.c_str()); } }
void pcl_ros::PCDReader::onInit () { ros::NodeHandle private_nh = getMTPrivateNodeHandle (); // Provide a latched topic ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true); private_nh.getParam ("publish_rate", publish_rate_); private_nh.getParam ("tf_frame", tf_frame_); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - publish_rate : %f\n" " - tf_frame : %s", getName ().c_str (), publish_rate_, tf_frame_.c_str ()); PointCloud2Ptr output_new; output_ = boost::make_shared<PointCloud2> (); output_new = boost::make_shared<PointCloud2> (); // Wait in a loop until someone connects do { ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ()); ros::spinOnce (); ros::Duration (0.01).sleep (); } while (private_nh.ok () && pub_output.getNumSubscribers () == 0); std::string file_name; while (private_nh.ok ()) { // Get the current filename parameter. If no filename set, loop if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ()) { ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); ros::Duration (0.01).sleep (); ros::spinOnce (); continue; } // If the filename parameter holds a different value than the last one we read if (file_name_.compare (file_name) != 0 && !file_name_.empty ()) { NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); file_name = file_name_; PointCloud2 cloud; if (impl_.read (file_name_, cloud) < 0) { NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); return; } output_ = boost::make_shared<PointCloud2> (cloud); output_->header.stamp = ros::Time::now (); output_->header.frame_id = tf_frame_; } // We do not publish empty data if (output_->data.size () == 0) continue; if (publish_rate_ == 0) { if (output_ != output_new) { NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); pub_output.publish (output_); output_new = output_; } ros::Duration (0.01).sleep (); } else { NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); output_->header.stamp = ros::Time::now (); pub_output.publish (output_); ros::Duration (publish_rate_).sleep (); } ros::spinOnce (); // Update parameters from server private_nh.getParam ("publish_rate", publish_rate_); private_nh.getParam ("tf_frame", tf_frame_); } }
int main(int argc, char **argv) { //---------------------------------------------------------------- //Init //---------------------------------------------------------------- uav_detected = false; ros::init(argc, argv, "control_node"); ros::NodeHandle nh; ros::Rate rate(updateRate); //Create subscribers and publichers// #ifdef SIMULATION //Subscripe to MAVROS local pose ros::Subscriber estimated_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose",1,pose_cb); #else //Subscribe to pose estimater ros::Subscriber estimated_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/mavros/vision_pose/pose_cov",1,pose_cb); #endif //Subscribe to UAV state ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb); //Subscribe to UAV extended sshift + 'D'tate ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>("mavros/extended_state", 10, extended_state_cb); //Create published for publiching setpoints local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",1); //Queue set to one (Allways publich the newest!) //Create service client for setting UAV state set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode"); //init boolean for streaming commands enabled/disabled ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/land"); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); mavros_msgs::CommandBool disarm_cmd; disarm_cmd.request.value = false; mavros_msgs::CommandBool land_cmd; land_cmd.request.value = true; streaming_setpoints = false; //---------------------------------------------------------------- //Wait for FCU connection and for the UAV to be armed //---------------------------------------------------------------- //wait for FCU connection ROS_INFO("Waiting for FCU connection"); while(current_state.connected) { ros::spinOnce(); rate.sleep(); if(!ros::ok()) { ROS_INFO("Shutting down control node"); return 0; } } ROS_INFO("Connected to FCU"); //wait for UAV to be armed ROS_INFO("Wating for UAV to be armed"); while(!current_state.armed ) { ros::spinOnce(); rate.sleep(); if(!ros::ok()) { ROS_INFO("Shutting down control node"); return 0; } } ROS_INFO("UAV is armed"); //ROS_INFO("Press and hold \"space\" to trigger failsafe"); ROS_INFO("Press and hold \"space\" to trigger KILLSWITCH! (disarm immediately)"); //---------------------------------------------------------------- //State machine //---------------------------------------------------------------- int state = IDLE; int next_state = IDLE; bool statePrinted = false; while(ros::ok()) { if(state != next_state) { state = next_state; statePrinted = false; } int c = getch(); //if(32 == c) //{ // state = FAILSAFE; // next_state = FAILSAFE; //} if(32 == c) { state = KILLSWITCH; next_state = KILLSWITCH; } switch(state) { case IDLE://Wait for user input if(!statePrinted) { ROS_INFO("STATE: ILDE"); ROS_INFO_ONCE("Press and hold 's' to start system"); statePrinted = true; } if('s' == getch()) { next_state = SEARCHING; } break; case SEARCHING: if(!statePrinted) { ROS_INFO("STATE: SEARCHING"); ROS_INFO("Searching for UAV"); statePrinted = true; } //WAIT FOR UAV TO BE DETECTED if(uav_detected) ROS_INFO("UAV detected"); next_state = START_SETPOINT_STREAM; break; case START_SETPOINT_STREAM: if(!statePrinted) { ROS_INFO("STATE: START_SETPOINT_STREAM"); statePrinted = true; } if(start_setpoint_stream()) next_state = ENABLE_OFFBOARD; break; case ENABLE_OFFBOARD: if(!statePrinted) { ROS_INFO("STATE: ENABLE_OFFBOARD"); ROS_INFO("Enableling offboard mode"); statePrinted = true; } if(enable_offboard()) { ROS_INFO("Offboard mode enabled"); next_state = WAIT_FOR_POS_REACHED; } else ROS_INFO("Failed to enable offboard mode"); break; case WAIT_FOR_POS_REACHED: if(!statePrinted) { ROS_INFO("STATE: WAIT_FOR_POS_REACHED"); ROS_INFO("Waiting for UAV to center over platform"); statePrinted = true; } //Go to descend if within 0.05 meters of setpoint if(eu_dist_to_target_point() < MaxDistToTargetPoint) { ROS_INFO("UAV centered over platform"); next_state = DESCEND; } break; case DESCEND: if(!statePrinted) { ROS_INFO("STATE: DESCEND"); ROS_INFO("Staring to descend"); statePrinted = true; } if(z_dist() < DisarmHeigh) { ROS_INFO("Disarm height reached"); ROS_INFO("Disarming"); if( arming_client.call(disarm_cmd) && disarm_cmd.response.success){ next_state = DISARMED; } } else if(eu_dist_to_center() < MaxDistToCenterXYPlane) { descend(); } else { ROS_WARN("UAV moved away form center"); ROS_INFO("Retrying landing"); next_state = WAIT_FOR_POS_REACHED; } break; case DISARMED: if(!statePrinted) { ROS_INFO("STATE: DISARMED"); statePrinted = true; } if(extended_current_state.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND) { ROS_INFO("Landing detected"); next_state = LANDED; } break; case LANDED: if(!statePrinted) { ROS_INFO("STATE: LANDED"); ROS_INFO("Landing compleate"); ROS_WARN("Control_node shutting down"); statePrinted = true; return 0; } break; case FAILSAFE: //Land ROS_ERROR_ONCE("STOPPED STREAMING SETPOINT! THIS WILL TRIGGER THE FAILSAFE"); ROS_ERROR_ONCE("PRESS AND HOLD shift + 'D' TO DISARM UAV NOW!"); streaming_setpoints = false; if(extended_current_state.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND) { ROS_INFO_ONCE("Landing detected"); next_state = LANDED; } break; case KILLSWITCH: //DISARM THE UAV REGARDLESS OF STATE ROS_FATAL_ONCE("KILLSWITCH ENABLED! UAV WILL BE DISARMED IMMEDIATELY!"); if( arming_client.call(disarm_cmd) && disarm_cmd.response.success){ ROS_FATAL_ONCE("UAV DISARMED!"); } break; } if(streaming_setpoints) { ROS_INFO_ONCE("Streaming target positions"); local_pos_pub.publish(TargetPos); } ros::spinOnce(); rate.sleep(); } //---------------------------------------------------------------- //Shutdown //---------------------------------------------------------------- ROS_INFO("control_node shutting down"); return 0; }
void StereoUndistort::processAndSendImage( const sensor_msgs::ImageConstPtr& image_msg_in, const CameraSide& side) { cv_bridge::CvImageConstPtr image_in_ptr = cv_bridge::toCvShare(image_msg_in, output_image_type_); std::string encoding = image_in_ptr->encoding; if (encoding == "8UC1") { // ros does not recognize U8C1 and it will crash the disparity node encoding = "mono8"; } cv_bridge::CvImagePtr image_out_ptr( new cv_bridge::CvImage(image_in_ptr->header, encoding)); updateUndistorter(side); if (side == CameraSide::FIRST) { image_out_ptr->header.frame_id = first_output_frame_; first_undistorter_ptr_->undistortImage(image_in_ptr->image, &(image_out_ptr->image)); first_image_pub_.publish(*(image_out_ptr->toImageMsg())); if (publish_tf_) { Eigen::Matrix4d T = stereo_camera_parameters_ptr_->getFirst().getOutputPtr()->T(); tf::Matrix3x3 R_ros; tf::Vector3 p_ros; tf::matrixEigenToTF(T.topLeftCorner<3, 3>(), R_ros); tf::vectorEigenToTF(T.topRightCorner<3, 1>(), p_ros); tf::Transform(R_ros, p_ros); std::string frame = image_in_ptr->header.frame_id; if (rename_input_frame_) { frame = first_input_frame_; } if (frame.empty()) { ROS_ERROR_ONCE("Image frame name is blank, cannot construct tf"); } else { br_.sendTransform(tf::StampedTransform(tf::Transform(R_ros, p_ros), image_out_ptr->header.stamp, frame, first_output_frame_)); } } } else { image_out_ptr->header.frame_id = second_output_frame_; second_undistorter_ptr_->undistortImage(image_in_ptr->image, &(image_out_ptr->image)); second_image_pub_.publish(*(image_out_ptr->toImageMsg())); if (publish_tf_) { Eigen::Matrix4d T = stereo_camera_parameters_ptr_->getSecond().getOutputPtr()->T(); tf::Matrix3x3 R_ros; tf::Vector3 p_ros; tf::matrixEigenToTF(T.topLeftCorner<3, 3>(), R_ros); tf::vectorEigenToTF(T.topRightCorner<3, 1>(), p_ros); tf::Transform(R_ros, p_ros); std::string frame = image_in_ptr->header.frame_id; if (rename_input_frame_) { frame = second_input_frame_; } if (frame.empty()) { ROS_ERROR_ONCE("Image frame name is blank, cannot construct tf"); } else { br_.sendTransform(tf::StampedTransform(tf::Transform(R_ros, p_ros), image_out_ptr->header.stamp, frame, second_output_frame_)); } } } }