void initialize(UAS &uas_) { double conn_system_time_d; double conn_timesync_d; ros::Duration conn_system_time; ros::Duration conn_timesync; uas = &uas_; if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) { conn_system_time = ros::Duration(ros::Rate(conn_system_time_d)); } else if (nh.getParam("conn/system_time", conn_system_time_d)) { // XXX deprecated parameter ROS_WARN_NAMED("time", "TM: parameter `~conn/system_time` deprecated, " "please use `~conn/system_time_rate` instead!"); conn_system_time = ros::Duration(conn_system_time_d); } if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) { conn_timesync = ros::Duration(ros::Rate(conn_timesync_d)); } else if (nh.getParam("conn/timesync", conn_timesync_d)) { // XXX deprecated parameter ROS_WARN_NAMED("time", "TM: parameter `~conn/timesync` deprecated, " "please use `~conn/timesync_rate` instead!"); conn_timesync = ros::Duration(conn_timesync_d); } nh.param<std::string>("time/time_ref_source", time_ref_source, "fcu"); nh.param("time/timesync_avg_alpha", offset_avg_alpha, 0.6); /* * alpha for exponential moving average. The closer alpha is to 1.0, * the faster the moving average updates in response to new offset samples (more jitter) * We need a significant amount of smoothing , more so for lower message rates like 1Hz */ time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10); // timer for sending system time messages if (!conn_system_time.isZero()) { sys_time_timer = nh.createTimer(conn_system_time, &SystemTimePlugin::sys_time_cb, this); sys_time_timer.start(); } // timer for sending timesync messages if (!conn_timesync.isZero()) { // enable timesync diag only if that feature enabled UAS_DIAG(uas).add(dt_diag); timesync_timer = nh.createTimer(conn_timesync, &SystemTimePlugin::timesync_cb, this); timesync_timer.start(); } }
MovePlatformAction() : as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER ac_planner_("/plan_action", true), // Planner action CLIENT ac_control_("/control_action", true) // Controller action CLIENT { n_.param("/move_platform_server/debug", debug_, false); std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server name = name + ".debug"; logger_name_ = "debug"; //logger is ros.carlos_motion_action_server.debug if (debug_) { // if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!! // so for debug we'll use a named logger if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name ros::console::notifyLoggerLevelsChanged(); } else // if not DEBUG we want INFO { if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name ros::console::notifyLoggerLevelsChanged(); } ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server"); as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this)); as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this)); //start the move server as_.start(); ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started"); // now wait for the other servers (planner + controller) to start ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start"); ac_planner_.waitForServer(); ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected()); ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start"); ac_control_.waitForServer(); ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected()); n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ); state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this); state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1); planning_ = false; controlling_ = false; //set_terminal_state_; ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this); planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this); }
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal if ( ! costmap_ros_->getRobotPose(current_pose_)) { ROS_ERROR("Could not get robot pose"); return false; } std::vector<geometry_msgs::PoseStamped> transformed_plan; if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) { ROS_ERROR("Could not get local plan"); return false; } //if the global plan passed in is empty... we won't do anything if(transformed_plan.empty()) { ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan."); return false; } ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size()); //we want to clear the robot footprint from the costmap we're using costmap_ros_->clearRobotFootprint(); // make sure to update the costmap we'll use for this cycle costmap_ros_->getCostmapCopy(costmap_); // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan); if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) { //publish an empty plan because we've reached our goal position std::vector<geometry_msgs::PoseStamped> local_plan; std::vector<geometry_msgs::PoseStamped> transformed_plan; publishGlobalPlan(transformed_plan); publishLocalPlan(local_plan); base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits(); return latchedStopRotateController_.computeVelocityCommandsStopRotate( cmd_vel, limits.getAccLimits(), dp_->getSimPeriod(), &planner_util_, odom_helper_, current_pose_, boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3)); } else { bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel); if (isOk) { publishGlobalPlan(transformed_plan); } else { ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path."); std::vector<geometry_msgs::PoseStamped> empty_plan; publishGlobalPlan(empty_plan); } return isOk; } }
inline bool is_normalized(float throttle, const float min, const float max) { if (throttle < min) { ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) < Min(%f)", throttle, min); return false; } else if (throttle > max) { ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) > Max(%f)", throttle, max); return false; } return true; }
// If robot has e--top button pressed, print a warning and return true. Otherwise return false. bool check_estop(const char *s) { arnl.robot->lock(); bool e = arnl.robot->isEStopPressed(); arnl.robot->unlock(); if(e) ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Warning: Robot e-stop button pressed, cannot %s", s); return e; }
void write() { if(!initted) { ROS_WARN_NAMED(APPLICATION_NAME, "hw interface was not initted yet (not able to connect to the robot), cannot send command to the robot\n"); return; } // write only meaningful values, if possible. printf("out: - size of controlledJoints is %d\n", (int) controlledJoints.size()); if(controlledJoints.size() == 0) return; msg_clear(); for(int i=0; i<controlledJoints.size(); i++) { output_msg.names.push_back(controlledJoints[i].first); output_msg.referenceType.push_back(controlledJoints[i].second); output_msg.reference.push_back(outCmd[jointMap.at(controlledJoints[i].first)]); printf("[%2d - %s] mode: %d; cmd: %f [%f]\n", i, controlledJoints[i].first.c_str(), output_msg.referenceType[i], output_msg.reference[i], outCmd[i]); } printf("\n"); fflush(stdout); yarpRosCmd_publisher.publish(output_msg); }
void autopilot_version_cb(const ros::TimerEvent &event) { bool ret = false; try { auto client = nh.serviceClient<mavros::CommandLong>("cmd/command"); mavros::CommandLong cmd{}; cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES; cmd.request.confirmation = false; cmd.request.param1 = 1.0; ROS_DEBUG_NAMED("sys", "VER: Sending request."); ret = client.call(cmd); } catch (ros::InvalidNameException &ex) { ROS_ERROR_NAMED("sys", "VER: %s", ex.what()); } ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!"); if (version_retries > 0) { version_retries--; ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys", "VER: request timeout, retries left %d", version_retries); } else { uas->update_capabilities(false); autopilot_version_timer.stop(); ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, " "switched to default capabilities"); } }
DoorTraj::~DoorTraj() { last_traj_id = id; if (pthread_create(&recorder_thread,NULL,&write_recorder_data,(void*)recorder)) { ROS_WARN_NAMED("OpenDoor","Could not create thread to write log data"); } hybridplug->current_traj=NULL; }
void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result) { controlling_ = false; ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString()); move_result_.result_state = result->result_state; move_result_.error_string = result->error_string; if (move_result_.result_state) { as_.setSucceeded(move_result_); ROS_INFO_NAMED(logger_name_, "Goal was successful :)"); } else { ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)"); // if is preempted => as_ was already set, cannot set again if (state.toString() != "PREEMPTED") { as_.setAborted(move_result_); ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted"); } else { if (set_terminal_state_) { as_.setPreempted(move_result_); ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted"); } } } }
/*! \brief Sets planning scene based on the stored list of objects and allowed contact region and returns the scene.*/ arm_navigation_msgs::SetPlanningSceneDiff::Response collisionObjects::setPlanningScene(){ arm_navigation_msgs::SetPlanningSceneDiff::Request req; arm_navigation_msgs::SetPlanningSceneDiff::Response res; for(unsigned int i=0; i<_att_obj.size(); i++){ if(!_att_obj[i].object.shapes.empty()){ req.planning_scene_diff.attached_collision_objects.push_back(_att_obj[i]); } } for(unsigned int i=0; i<_coll_obj.size(); i++){ if(!_coll_obj[i].shapes.empty()){ req.planning_scene_diff.collision_objects.push_back(_coll_obj[i]); } } if(!_allowed_contact.shape.dimensions.empty()){ req.planning_scene_diff.allowed_contacts.push_back(_allowed_contact); } if(!_set_pln_scn_client.call(req, res)){ ROS_ERROR_NAMED(CL_LGRNM,"Call to %s failed", SET_PLANNING_SCENE_DIFF_NAME); if(!ros::service::exists(SET_PLANNING_SCENE_DIFF_NAME, true)){ ROS_WARN_NAMED(CL_LGRNM,"%s service doesn't exist", SET_PLANNING_SCENE_DIFF_NAME); } } //printListOfObjects(); return res; }
bool stop(Stop::Request &req, Stop::Response &res){ KukaRequest kuka_req( 10, STOP); //(priority, SRV) addRequest(kuka_req); ROS_WARN_NAMED(name_, "Stop command received."); res.result = true; return true; }
bool RosArnlNode::global_localization_srv_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Localize init (global_localization service) request..."); // arnl.locTask->localizeRobotInMapInit(); if(! arnl.locTask->localizeRobotAtHomeBlocking() ) ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Error in initial localization."); return true; }
void Navigation::doneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) { if( move_base_ac_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_WARN_NAMED(name_, "Move base failed to approach target"); move_base_ac_.cancelAllGoals(); move_base_msgs::MoveBaseGoal goal_msg; ROS_INFO_NAMED(name_,"GOING TO recovery pose: %i", BOX_CHARGE); goal_msg.target_pose.pose = recovery_; sendMoveBaseGoal(goal_msg); in_recovery_mode = true; //result_.state = free_navigation::NavigateFreelyResult::FAILED; //as_.setAborted(result_,"Move base failed to approach target"); return; } // if at staging area then approach previous goal again if(in_recovery_mode) { in_recovery_mode = false; approachGoal(); } else { //ROS_INFO_NAMED(name_,"Finished in state: %s", state.getText().c_str()); switch (goal_) { case CHARGE: ROS_INFO_NAMED(name_,"Docking in CHARGER: %i", BOX_CHARGE); { docking_with_walls::docking_with_wallsGoal dock_goal; docking_with_walls_ac_.sendGoal(dock_goal, boost::bind(&Navigation::doneCbWalls,this,_1,_2)); } break; case BRICK: ROS_INFO_NAMED(name_,"going in to collect bricks: %i", BRICK); { collect_bricks_pos::collect_bricks_posGoal collect_bricks_goal; collect_bricks_ac_.sendGoal(collect_bricks_goal, boost::bind(&Navigation::doneCollectBricksCv,this,_1,_2)); } break; case DELIVERY: ROS_INFO_NAMED(name_,"Tipping of at DELIVERY: %i", DELIVERY); ros::Duration(2.0).sleep(); as_.setSucceeded(result_); current_position = Navigation::free; break; case TRANSITION: ROS_INFO_NAMED(name_,"At transition: %i", TRANSITION); as_.setSucceeded(result_); current_position = Navigation::at_transition; break; default: //ac_.cancelAllGoals(); return; // not a navigation command so do not navigate } } }
/** * @brief Function to verify if the thrust values are normalized; * considers also the reversed trust values */ inline bool is_normalized(float thrust){ if (reverse_thrust) { if (thrust < -1.0) { ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0); return false; } } else { if (thrust < 0.0) { ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0); return false; } } if (thrust > 1.0) { ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0); return false; } return true; }
void QuadrotorPropulsion::addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm) { boost::mutex::scoped_lock lock(command_queue_mutex_); if (!motor_status_.on) { ROS_WARN_NAMED("quadrotor_propulsion", "Received new motor command. Enabled motors."); engage(); } ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Received motor command valid at " << pwm->header.stamp); command_queue_.push(pwm); command_condition_.notify_all(); }
void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) { if (snap < 0.0 || snap > 1.0) ROS_WARN_NAMED("model_based_state_space", "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " "Value remains as previously set (%lf)", tag_snap_to_segment_); else { tag_snap_to_segment_ = snap; tag_snap_to_segment_complement_ = 1.0 - tag_snap_to_segment_; } }
bool read() { printf("In:\n"); int msgLenght; if(initted) { mutex.lock(); msgLenght = state_msg->name.size(); if(msgLenght <= 0) { ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message of size %d", msgLenght); return false; } // size of msg can be smaller than number of joints, but not bigger if(msgLenght > joint_num) { ROS_ERROR_NAMED(APPLICATION_NAME, "Size of input data is %d, which is bigger than numer of joints [%d]", msgLenght, joint_num); return false; } if(state_msg->position.size() != msgLenght) { ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message. Size of 'names' is different from size of references."); return false; } // same check for other fields?? TBD: define this f*****g msg!!! // Joints can be in any order, so we need a map to fill in the right spot. for(int i=0; i<msgLenght; i++) { // TODO: using '[]' operator may cause issues, use '.at()' instead measPos[jointMap[state_msg->name[i]]] = state_msg->position[i]; measVel[jointMap[state_msg->name[i]]] = state_msg->velocity[i]; measEff[jointMap[state_msg->name[i]]] = state_msg->effort[i]; printf("[%2d] %-*s %8.4f\t%8.4f\t%8.4f\n", i, longestJointName_size+2, state_msg->name[i].c_str(), measPos[i], measVel[i], measEff[i]); } ROS_WARN_NAMED(APPLICATION_NAME, "TBS: Try to get the most recent reference target maybe??\n"); mutex.unlock(); fflush(stdout); } else { std::cout << "Not initted yet" << std::endl; } return true; }
void checkImagesSynchronized() { int threshold = 3 * both_received_; if (image_received_ > threshold || info_received_ > threshold) { ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else "[image_transport] Topics '%s' and '%s' do not appear to be synchronized. " "In the last 10s:\n" "\tImage messages received: %d\n" "\tCameraInfo messages received: %d\n" "\tSynchronized pairs: %d", image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), image_received_, info_received_, both_received_); } image_received_ = info_received_ = both_received_ = 0; }
void convert_to_bus(SL_Bus_etasetgett_std_msgs_MultiArrayLayout* busPtr, std_msgs::MultiArrayLayout const* msgPtr) { busPtr->DataOffset = msgPtr->data_offset; { int numItemsToCopy = msgPtr->dim.size(); if (numItemsToCopy > 16) { ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "dim", "std_msgs/MultiArrayLayout", numItemsToCopy, 16); numItemsToCopy = 16; } busPtr->Dim_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy ); for (int i=0; i < numItemsToCopy; i++) { convert_to_bus(&busPtr->Dim[i], &msgPtr->dim[i]); } } }
void ariaLogHandler(const char *msg, ArLog::LogLevel level) { // node that ARIA logging is normally limited at Normal and Terse only. Set // ARLOG_LEVEL environment variable to override. switch(level) { case ArLog::Normal: ROS_INFO_NAMED("ARNL", "ARNL: %s", msg); return; case ArLog::Terse: ROS_WARN_NAMED("ARNL", "ARNL: %s", msg); return; case ArLog::Verbose: ROS_DEBUG_NAMED("ARNL", "ARNL: %s", msg); return; } }
void convert_to_bus(SL_Bus_etasetgett_std_msgs_Float32MultiArray* busPtr, std_msgs::Float32MultiArray const* msgPtr) { { int numItemsToCopy = msgPtr->data.size(); if (numItemsToCopy > 256) { ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "data", "std_msgs/Float32MultiArray", numItemsToCopy, 256); numItemsToCopy = 256; } busPtr->Data_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy ); for (int i=0; i < numItemsToCopy; i++) { busPtr->Data[i] = msgPtr->data[i]; } } convert_to_bus(&busPtr->Layout, &msgPtr->layout); }
void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_msgs::PickupGoalConstPtr& input_goal) { setPickupState(PLANNING); // before we start planning, ensure that we have the latest robot state received... context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); context_->planning_scene_monitor_->updateFrameTransforms(); moveit_msgs::PickupGoalConstPtr goal; if (input_goal->possible_grasps.empty()) { moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal)); goal.reset(copy); fillGrasps(*copy); } else goal = input_goal; moveit_msgs::PickupResult action_res; if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_) { if (!goal->planning_options.plan_only) ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick " "goal request has plan_only set to false. Only a motion plan will be computed " "anyway."); executePickupCallbackPlanOnly(goal, action_res); } else executePickupCallbackPlanAndExecute(goal, action_res); bool planned_trajectory_empty = action_res.trajectory_stages.empty(); std::string response = getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only); if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) pickup_action_server_->setSucceeded(action_res, response); else { if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED) pickup_action_server_->setPreempted(action_res, response); else pickup_action_server_->setAborted(action_res, response); } setPickupState(IDLE); }
void convert_to_bus(SL_Bus_etasetgett_std_msgs_MultiArrayDimension* busPtr, std_msgs::MultiArrayDimension const* msgPtr) { { int numItemsToCopy = msgPtr->label.size(); if (numItemsToCopy > 128) { ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "label", "std_msgs/MultiArrayDimension", numItemsToCopy, 128); numItemsToCopy = 128; } busPtr->Label_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy ); for (int i=0; i < numItemsToCopy; i++) { busPtr->Label[i] = (uint8_T) msgPtr->label[i]; } } busPtr->Size = msgPtr->size; busPtr->Stride = msgPtr->stride; }
void convert_to_bus(SL_Bus_Hector2P2_std_msgs_Header* busPtr, std_msgs::Header const* msgPtr) { { int numItemsToCopy = msgPtr->frame_id.size(); if (numItemsToCopy > 128) { ROS_WARN_NAMED("Hector2P2", "Truncating array '%s' in received message '%s' from %d to %d items", "frame_id", "std_msgs/Header", numItemsToCopy, 128); numItemsToCopy = 128; } busPtr->FrameId_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy ); for (int i=0; i < numItemsToCopy; i++) { busPtr->FrameId[i] = (uint8_T) msgPtr->frame_id[i]; } } busPtr->Seq = msgPtr->seq; convert_to_bus(&busPtr->Stamp, &msgPtr->stamp); }
void initialize(UAS &uas_) { uas = &uas_; XmlRpc::XmlRpcValue map_dict; if (!dist_nh.getParam("", map_dict)) { ROS_WARN_NAMED("distance_sensor", "DS: plugin not configured!"); return; } ROS_ASSERT(map_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct); for (auto &pair : map_dict) { ROS_DEBUG_NAMED("distance_sensor", "DS: initializing mapping for %s", pair.first.c_str()); auto it = DistanceSensorItem::create_item(this, pair.first); if (it) sensor_map[it->sensor_id] = it; else ROS_ERROR_NAMED("distance_sensor", "DS: bad config for %s", pair.first.c_str()); } }
ompl_interface::ConstraintApproximationConstructionResults ompl_interface::ConstraintsLibrary::addConstraintApproximation( const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, const std::string& group, const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options) { ConstraintApproximationConstructionResults res; ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization); if (pc) { pc->clear(); pc->setPlanningScene(scene); pc->setCompleteInitialState(scene->getCurrentState()); ros::WallTime start = ros::WallTime::now(); ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res); ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec()); if (ss) { ConstraintApproximationPtr ca(new ConstraintApproximation( group, options.state_space_parameterization, options.explicit_motions, constr_hard, group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss, res.milestones)); if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end()) ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str()); constraint_approximations_[ca->getName()] = ca; res.approx = ca; } else ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'", group.c_str()); } return res; }
bool GazeboNoisyDepth::FillDepthImageHelper(const uint32_t rows_arg, const uint32_t cols_arg, const uint32_t step_arg, const float *data_arg, sensor_msgs::Image *image_msg) { if(data_arg == nullptr){ ROS_WARN_NAMED("NoisyDepth", "Invalid data array received - nullptr."); return false; } image_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; image_msg->height = rows_arg; image_msg->width = cols_arg; image_msg->step = sizeof(float) * cols_arg; image_msg->data.resize(rows_arg * cols_arg * sizeof(float)); image_msg->is_bigendian = 0; float *dest = (float *)(&(image_msg->data[0])); memcpy(dest, data_arg, sizeof(float) * width * height); noise_model->ApplyNoise(width, height, dest); return true; }
void OpenNIListener::processBagfile(std::string filename, const std::string& visua_tpc, const std::string& depth_tpc, const std::string& points_tpc, const std::string& cinfo_tpc, const std::string& odom_tpc, const std::string& tf_tpc) { ROS_INFO_NAMED("OpenNIListener", "Loading Bagfile %s", filename.c_str()); Q_EMIT iamBusy(4, "Loading Bagfile", 0); { //bag will be destructed after this block (hopefully frees memory for the optimizer) rosbag::Bag input_bag; try{ input_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_NAMED("OpenNIListener", "Opened Bagfile %s", filename.c_str()); std::vector<std::string> topics; topics = createTopicsVector(visua_tpc, depth_tpc, points_tpc, cinfo_tpc, odom_tpc, tf_tpc); rosbag::View view(input_bag, rosbag::TopicQuery(topics)); Q_EMIT iamBusy(4, "Processing Bagfile", view.size()); // Simulate sending of the messages in the bagfile std::deque<sensor_msgs::Image::ConstPtr> vis_images; std::deque<sensor_msgs::Image::ConstPtr> dep_images; std::deque<sensor_msgs::CameraInfo::ConstPtr> cam_infos; std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds; std::deque<nav_msgs::OdometryConstPtr> odometries; //ros::Time last_tf=ros::Time(0); ros::Time last_tf=ros::TIME_MIN; bool tf_available = false; int counter = 0; BOOST_FOREACH(rosbag::MessageInstance const m, view) { Q_EMIT progress(4, "Processing Bagfile", counter++); do{ usleep(10); if(!ros::ok()) return; } while(pause_); ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec()); if (m.getTopic() == odom_tpc || ("/" + m.getTopic() == odom_tpc)) { ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec()); nav_msgs::OdometryConstPtr odommsg = m.instantiate<nav_msgs::Odometry>(); if (odommsg) odometries.push_back(odommsg); } else if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc)) { sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>(); if (rgb_img) vis_images.push_back(rgb_img); ROS_DEBUG("Found Message of %s", visua_tpc.c_str()); } else if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc)) { sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>(); //if (depth_img) depth_img_sub_->newMessage(depth_img); if (depth_img) dep_images.push_back(depth_img); ROS_DEBUG("Found Message of %s", depth_tpc.c_str()); } else 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_DEBUG("Found Message of %s", points_tpc.c_str()); } else if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc)) { sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>(); //if (cam_info) cam_info_sub_->newMessage(cam_info); if (cam_info) cam_infos.push_back(cam_info); ROS_DEBUG("Found Message of %s", cinfo_tpc.c_str()); } else if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){ tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>(); if (tf_msg) { tf_available = true; addTFMessageDirectlyToTransformer(tf_msg, tflistener_); last_tf = tf_msg->transforms[0].header.stamp; last_tf -= ros::Duration(0.1); } } if (last_tf == ros::TIME_MIN){//If not a valid time yet, set to something before first message's stamp last_tf = m.getTime(); last_tf -= ros::Duration(0.1); } //last_tf = m.getTime();//FIXME: No TF -> no processing while(!odometries.empty() && odometries.front()->header.stamp < last_tf){ ROS_INFO_NAMED("OpenNIListener", "Sending Odometry message"); odomCallback(odometries.front()); odometries.pop_front(); } while(!vis_images.empty() && vis_images.front()->header.stamp < last_tf){ ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered visual message from time %12f", vis_images.front()->header.stamp.toSec()); rgb_img_sub_->newMessage(vis_images.front()); vis_images.pop_front(); } while(!dep_images.empty() && dep_images.front()->header.stamp < last_tf){ ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered depth message from time %12f", dep_images.front()->header.stamp.toSec()); depth_img_sub_->newMessage(dep_images.front()); dep_images.pop_front(); } while(!cam_infos.empty() && cam_infos.front()->header.stamp < last_tf){ ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered cam info message from time %12f", cam_infos.front()->header.stamp.toSec()); cam_info_sub_->newMessage(cam_infos.front()); cam_infos.pop_front(); } while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){ pc_sub_->newMessage(pointclouds.front()); pointclouds.pop_front(); } } ROS_WARN_NAMED("eval", "Finished processing of Bagfile"); input_bag.close(); }
void sendCommad(){ ros::Duration sleep_time(0.1); bool error = false; unsigned int num_req; while(nh_.ok() && ros::ok() && !error){ num_req = getRequestNum(); if (num_req > 0){ KukaRequest req = getRequest(); pop(); // TODO // Check option switch (req.opt_){ case STOP: // Send Stop error = kuka_.stop(); ROS_WARN_NAMED(name_, "Sending stop request to KUKA robot."); break; case PTP: // Send PTP command kuka_.setJointPosition(req.data_.ptp_goal); ROS_DEBUG_NAMED(name_, "Sending PTP request to KUKA robot."); break; case VEL: // Send PTP command kuka_.setOverrideSpeed(req.data_.vel); ROS_DEBUG_NAMED(name_, "Sending velocity override request to KUKA robot."); break; // case HOME: // // Send Home // for(unsigned int i=0; i<6; i++) req.data_.ptp_goal[i]=0.0*rad2deg+offset_[i]; // kuka_.setJointPosition(req.data_.ptp_goal); // ROS_DEBUG_NAMED(name_, "Sending HOME request to KUKA robot."); // break; default: ROS_ERROR_NAMED(name_, "Undefined option type."); } sleep_time = ros::Duration(0.1); } // Update joint state else if (num_req == 0) { ROS_DEBUG_NAMED(name_, "Update request to KUKA robot."); // Call KUKA update kuka_.update(); // Reset delta position and norm values double norm = 0.0; for (unsigned int i = 0; i < 6; ++i) position_d_[i] = joint_state_.position[i]; // Get joint position kuka_.getJointPosition(joint_state_.position); // Offset and rad convertion for (unsigned int i = 0; i < 6; ++i) { joint_state_.position[i] = (joint_state_.position[i] - offset_[i])*deg2rad; position_d_[i] -= joint_state_.position[i]; norm += position_d_[i]*position_d_[i]; } //norm = sqrt(norm); if ( norm < 0.01 ) sleep_time = ros::Duration(1); ROS_DEBUG_NAMED(name_, "Norma: %.2f", norm); // Publish joint state joint_state_.header.stamp = ros::Time::now(); joint_state_.header.seq++; joint_state_pub_.publish(joint_state_); } ROS_DEBUG_THROTTLE_NAMED(0.2, name_, "Current number of req %i", getRequestNum()); sleep_time.sleep(); } }
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation( const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, ConstraintApproximationConstructionResults& result) { // state storage structure ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace()); ob::StateStoragePtr sstor(cass); // construct a sampler for the sampling constraints kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel()); robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame()); kset.add(constr_hard, no_transforms); const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState(); unsigned int attempts = 0; double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0; pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val, bounds_val); pcontext->getOMPLStateSpace()->setup(); // construct the constrained states robot_state::RobotState robot_state(default_state); const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager(); ConstrainedSampler* csmp = nullptr; if (csmng) { constraint_samplers::ConstraintSamplerPtr cs = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling); if (cs) csmp = new ConstrainedSampler(pcontext.get(), cs); } ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler()); ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace()); int done = -1; bool slow_warn = false; ompl::time::point start = ompl::time::now(); while (sstor->size() < options.samples) { ++attempts; int done_now = 100 * sstor->size() / options.samples; if (done != done_now) { done = done_now; ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)sstor->size() / (double)attempts); } if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100) { slow_warn = true; ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow..."); } if (attempts > options.samples && sstor->size() == 0) { ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples"); break; } ss->sampleUniform(temp.get()); pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get()); if (kset.decide(robot_state).satisfied) { if (sstor->size() < options.samples) { temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size(); sstor->addState(temp.get()); } } } result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start); ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(), result.state_sampling_time); if (csmp) { result.sampling_success_rate = csmp->getConstrainedSamplingRate(); ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate); } result.milestones = sstor->size(); if (options.edges_per_sample > 0) { ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); // construct connexions const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); unsigned int milestones = sstor->size(); std::vector<ob::State*> int_states(options.max_explicit_points, nullptr); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states); ompl::time::point start = ompl::time::now(); int good = 0; int done = -1; for (std::size_t j = 0; j < milestones; ++j) { int done_now = 100 * j / milestones; if (done != done_now) { done = done_now; ROS_INFO_NAMED("constraints_library", "%d%% complete", done); } if (cass->getMetadata(j).first.size() >= options.edges_per_sample) continue; const ob::State* sj = sstor->getState(j); for (std::size_t i = j + 1; i < milestones; ++i) { if (cass->getMetadata(i).first.size() >= options.edges_per_sample) continue; double d = space->distance(sstor->getState(i), sj); if (d >= options.max_edge_length) continue; unsigned int isteps = std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution); double step = 1.0 / (double)isteps; bool ok = true; space->interpolate(sstor->getState(i), sj, step, int_states[0]); for (unsigned int k = 1; k < isteps; ++k) { double this_step = step / (1.0 - (k - 1) * step); space->interpolate(int_states[k - 1], sj, this_step, int_states[k]); pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, int_states[k]); if (!kset.decide(robot_state).satisfied) { ok = false; break; } } if (ok) { cass->getMetadata(i).first.push_back(j); cass->getMetadata(j).first.push_back(i); if (options.explicit_motions) { cass->getMetadata(i).second[j].first = sstor->size(); for (unsigned int k = 0; k < isteps; ++k) { int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1; sstor->addState(int_states[k]); } cass->getMetadata(i).second[j].second = sstor->size(); cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j]; } good++; if (cass->getMetadata(j).first.size() >= options.edges_per_sample) break; } } } result.state_connection_time = ompl::time::seconds(ompl::time::now() - start); ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); return sstor; } // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic // Update with more intelligent logic as needed ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here."); return sstor; }