void JointTrajectoryStreamer::streamingThread() { int connectRetryCount = 1; ROS_INFO("Starting joint trajectory streamer thread"); while (ros::ok()) { ros::Duration(0.005).sleep(); // automatically re-establish connection, if required if (connectRetryCount-- > 0) { ROS_INFO("Connecting to robot motion server"); this->connection_->makeConnect(); ros::Duration(0.250).sleep(); // wait for connection if (this->connection_->isConnected()) connectRetryCount = 0; else if (connectRetryCount <= 0) { ROS_ERROR("Timeout connecting to robot controller. Send new motion command to retry."); this->state_ = TransferStates::IDLE; } continue; } this->mutex_.lock(); SimpleMessage msg, tmpMsg, reply; switch (this->state_) { case TransferStates::IDLE: ros::Duration(0.250).sleep(); // slower loop while waiting for new trajectory break; case TransferStates::STREAMING: if (this->current_point_ >= (int)this->current_traj_.size()) { ROS_INFO("Trajectory streaming complete, setting state to IDLE"); this->state_ = TransferStates::IDLE; break; } if (!this->connection_->isConnected()) { ROS_DEBUG("Robot disconnected. Attempting reconnect..."); connectRetryCount = 5; break; } tmpMsg = this->current_traj_[this->current_point_]; msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID, tmpMsg.getData()); // set commType=REQUEST ROS_DEBUG("Sending joint trajectory point"); if (this->connection_->sendAndReceiveMsg(msg, reply, false)) { ROS_INFO("Point[%d of %d] sent to controller", this->current_point_, (int)this->current_traj_.size()); this->current_point_++; } else ROS_WARN("Failed sent joint point, will try again"); break; default: ROS_ERROR("Joint trajectory streamer: unknown state"); this->state_ = TransferStates::IDLE; break; } this->mutex_.unlock(); } ROS_WARN("Exiting trajectory streamer thread"); }
virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector<std::size_t> &added_path_index) const { ROS_DEBUG("Running '%s'", getDescription().c_str()); // get the specified start state robot_state::RobotState start_state = planning_scene->getCurrentState(); robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); // if the start state is otherwise valid but does not meet path constraints if (planning_scene->isStateValid(start_state, req.group_name) && !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name)) { ROS_INFO("Path constraints not satisfied for start state..."); planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true); ROS_INFO("Planning to path constraints..."); planning_interface::MotionPlanRequest req2 = req; req2.goal_constraints.resize(1); req2.goal_constraints[0] = req.path_constraints; req2.path_constraints = moveit_msgs::Constraints(); planning_interface::MotionPlanResponse res2; // we call the planner for this additional request, but we do not want to include potential // index information from that call std::vector<std::size_t> added_path_index_temp; added_path_index_temp.swap(added_path_index); bool solved1 = planner(planning_scene, req2, res2); added_path_index_temp.swap(added_path_index); if (solved1) { planning_interface::MotionPlanRequest req3 = req; ROS_INFO("Planned to path constraints. Resuming original planning request."); // extract the last state of the computed motion plan and set it as the new start state robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); bool solved2 = planner(planning_scene, req3, res); res.planning_time_ += res2.planning_time_; if (solved2) { // since we add a prefix, we need to correct any existing index positions for (std::size_t i = 0 ; i < added_path_index.size() ; ++i) added_path_index[i] += res2.trajectory_->getWayPointCount(); // we mark the fact we insert a prefix path (we specify the index position we just added) for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i) added_path_index.push_back(i); // we need to append the solution paths. res2.trajectory_->append(*res.trajectory_, 0.0); res2.trajectory_->swap(*res.trajectory_); return true; } else return false; } else { ROS_WARN("Unable to plan to path constraints. Running usual motion plan."); bool result = planner(planning_scene, req, res); res.planning_time_ += res2.planning_time_; return result; } } else { ROS_DEBUG("Path constraints are OK. Running usual motion plan."); return planner(planning_scene, req, res); } }
void noFilterCallback (const sensor_msgs::PointCloud2ConstPtr &cloud) { pointCloudPublisher_.publish (cloud); ROS_DEBUG ("Self filter publishing unfiltered frame"); }
void set_speed_callback(const drive::speed::ConstPtr& msg) { driveChain.setMotorSpeeds((int)(msg->speedLeft * 1000000.0), (int)(msg->speedRight * 1000000.0)); ROS_DEBUG("set_speed_callback(): setting speeds %.2f / %.2f according to message on topic", msg->speedLeft, msg->speedRight); }
/* * One and only one callback, now takes cloud, does everything else needed. */ void ARPublisher::getTransformationCallback (const sensor_msgs::PointCloud2ConstPtr & msg) { sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image); ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int i, k, j; /* do we need to initialize? */ if(!configured_) { if(msg->width == 0 || msg->height == 0) { ROS_ERROR ("Deformed cloud! Size = %d, %d.", msg->width, msg->height); return; } cam_param_.xsize = msg->width; cam_param_.ysize = msg->height; cam_param_.dist_factor[0] = msg->width/2; // x0 = cX from openCV calibration cam_param_.dist_factor[1] = msg->height/2; // y0 = cY from openCV calibration cam_param_.dist_factor[2] = 0; // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2 cam_param_.dist_factor[3] = 1.0; // scale factor, should probably be >1, but who cares... arInit (); } /* convert cloud to PCL */ PointCloud cloud; pcl::fromROSMsg(*msg, cloud); /* get an OpenCV image from the cloud */ pcl::toROSMsg (cloud, *image_msg); try { capture_ = bridge_.imgMsgToCv (image_msg, "bgr8"); } catch (sensor_msgs::CvBridgeException & e) { ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ()); } dataPtr = (ARUint8 *) capture_->imageData; /* detect the markers in the video frame */ if (arDetectMarkerLite (dataPtr, threshold_, &marker_info, &marker_num) < 0) { argCleanup (); return; } arPoseMarkers_.markers.clear (); /* check for known patterns */ for (i = 0; i < objectnum; i++) { k = -1; for (j = 0; j < marker_num; j++) { if (object[i].id == marker_info[j].id) { if (k == -1) k = j; else // make sure you have the best pattern (highest confidence factor) if (marker_info[k].cf < marker_info[j].cf) k = j; } } if (k == -1) { object[i].visible = 0; continue; } /* create a cloud for marker corners */ int d = marker_info[k].dir; PointCloud marker; marker.push_back( cloud.at( (int)marker_info[k].vertex[(4-d)%4][0], (int)marker_info[k].vertex[(4-d)%4][1] ) ); // upper left marker.push_back( cloud.at( (int)marker_info[k].vertex[(5-d)%4][0], (int)marker_info[k].vertex[(5-d)%4][1] ) ); // upper right marker.push_back( cloud.at( (int)marker_info[k].vertex[(6-d)%4][0], (int)marker_info[k].vertex[(6-d)%4][1] ) ); // lower right marker.push_back( cloud.at( (int)marker_info[k].vertex[(7-d)%4][0], (int)marker_info[k].vertex[(7-d)%4][1] ) ); /* create an ideal cloud */ double w = object[i].marker_width; PointCloud ideal; ideal.push_back( makeRGBPoint(-w/2,w/2,0) ); ideal.push_back( makeRGBPoint(w/2,w/2,0) ); ideal.push_back( makeRGBPoint(w/2,-w/2,0) ); ideal.push_back( makeRGBPoint(-w/2,-w/2,0) ); /* get transformation */ Eigen::Matrix4f t; pcl::estimateRigidTransformationSVD( marker, ideal, t ); /* get final transformation */ tf::Transform transform = tfFromEigen(t.inverse()); // any(transform == nan) btMatrix3x3 m = transform.getBasis(); btVector3 p = transform.getOrigin(); bool invalid = false; for(int i=0; i < 3; i++) for(int j=0; j < 3; j++) invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0); for(int i=0; i < 3; i++) invalid = (invalid || isnan(p[i])); if(invalid) continue; /* publish the marker */ ar_pose::ARMarker ar_pose_marker; ar_pose_marker.header.frame_id = msg->header.frame_id; ar_pose_marker.header.stamp = msg->header.stamp; ar_pose_marker.id = object[i].id; ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX(); ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY(); ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ(); ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX(); ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY(); ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ(); ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW(); ar_pose_marker.confidence = marker_info->cf; arPoseMarkers_.markers.push_back (ar_pose_marker); /* publish transform */ if (publishTf_) { broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name)); } /* publish visual marker */ if (publishVisualMarkers_) { btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS); btTransform m (btQuaternion::getIdentity (), markerOrigin); btTransform markerPose = transform * m; // marker pose in the camera frame tf::poseTFToMsg (markerPose, rvizMarker_.pose); rvizMarker_.header.frame_id = msg->header.frame_id; rvizMarker_.header.stamp = msg->header.stamp; rvizMarker_.id = object[i].id; rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS; rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS; rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS; rvizMarker_.ns = "basic_shapes"; rvizMarker_.type = visualization_msgs::Marker::CUBE; rvizMarker_.action = visualization_msgs::Marker::ADD; switch (i) { case 0: rvizMarker_.color.r = 0.0f; rvizMarker_.color.g = 0.0f; rvizMarker_.color.b = 1.0f; rvizMarker_.color.a = 1.0; break; case 1: rvizMarker_.color.r = 1.0f; rvizMarker_.color.g = 0.0f; rvizMarker_.color.b = 0.0f; rvizMarker_.color.a = 1.0; break; default: rvizMarker_.color.r = 0.0f; rvizMarker_.color.g = 1.0f; rvizMarker_.color.b = 0.0f; rvizMarker_.color.a = 1.0; } rvizMarker_.lifetime = ros::Duration (); rvizMarkerPub_.publish (rvizMarker_); ROS_DEBUG ("Published visual marker"); } } arMarkerPub_.publish (arPoseMarkers_); ROS_DEBUG ("Published ar_multi markers"); }
void EncoderOdometryNode::publishEstimatedOdomMsg( const ros::TimerEvent& timer_event) { // Check to make sure that we have some counts for both the left and right // encoders if (!left_encoder_num_ticks_curr || !right_encoder_num_ticks_curr) { // If we don't have either the left or right encoder ticks, just return return; } // Checks if we've made an estimate before if (!left_encoder_num_ticks_prev || !right_encoder_num_ticks_prev) { ROS_DEBUG("Got first ticks from encoders"); // If we haven't, save the current state as the previous state // and just return. The first actual estimate will be made at the // next call to this function left_encoder_num_ticks_prev = left_encoder_num_ticks_curr; right_encoder_num_ticks_prev = right_encoder_num_ticks_curr; // Save the time so that next time we get ticks, we can // generate a state estimate last_estimate.header.stamp = ros::Time::now(); return; } // All math taken from: // https://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/16311/www/s07/labs/NXTLabs/Lab%203.html double dt = ros::Time::now().toSec() - last_estimate.header.stamp.toSec(); // left and right wheel velocities (in meters/s) int left_encoder_ticks = *left_encoder_num_ticks_curr - *left_encoder_num_ticks_prev; int right_encoder_ticks = *right_encoder_num_ticks_curr - *right_encoder_num_ticks_prev; double v_l = (left_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius; double v_r = (right_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius; // linear and angular velocity double v = (v_r + v_l) / 2; double w = (v_r - v_l) / wheelbase_length; // x and y velocity double x_vel = v * cos(w); double y_vel = v * sin(w); // position double prev_x = last_estimate.pose.pose.position.x; double prev_y = last_estimate.pose.pose.position.y; double prev_yaw = tf::getYaw(last_estimate.pose.pose.orientation); double k_00 = v * cos(prev_yaw); double k_01 = v * sin(prev_yaw); double k_02 = w; double k_10 = v * cos(prev_yaw + dt / 2 * k_02); double k_11 = v * sin(prev_yaw + dt / 2 * k_02); double k_12 = w; double k_20 = v * cos(prev_yaw + dt / 2 * k_12); double k_21 = v * sin(prev_yaw + dt / 2 * k_12); double k_22 = w; double k_30 = v * cos(prev_yaw + dt * k_22); double k_31 = v * sin(prev_yaw + dt * k_22); double k_32 = w; double x = prev_x + (dt / 6) * (k_00 + 2 * (k_10 + k_20) + k_30); double y = prev_y + (dt / 6) * (k_01 + 2 * (k_11 + k_21) + k_31); double yaw = prev_yaw + (dt / 6) * (k_02 + 2 * (k_12 + k_22) + k_32); // Update our estimate last_estimate.pose.pose.position.x = x; last_estimate.pose.pose.position.y = y; tf::quaternionTFToMsg(tf::createQuaternionFromYaw(yaw), last_estimate.pose.pose.orientation); last_estimate.twist.twist.linear.x = x_vel; last_estimate.twist.twist.linear.y = y_vel; last_estimate.twist.twist.angular.z = w; last_estimate.header.stamp = ros::Time::now(); last_estimate.header.frame_id = odom_frame_id; // Save the current number of ticks as the "previous" number for the // next time we estimate left_encoder_num_ticks_prev = left_encoder_num_ticks_curr; right_encoder_num_ticks_prev = right_encoder_num_ticks_curr; // Publish our estimate odom_estimate_publisher.publish(last_estimate); }
long long hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout) { ROS_DEBUG("Entering calcLatency."); if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); offset_ = 0; uint64_t comp_time = 0; uint64_t laser_time = 0; long long diff_time = 0; long long drift_time = 0; long long tmp_offset1 = 0; long long tmp_offset2 = 0; int count = 0; sendCmd("TM0",timeout); count = 100; for (int i = 0; i < count;i++) { usleep(1000); sendCmd("TM1",timeout); comp_time = timeHelper(); try { laser_time = readTime(); diff_time = comp_time - laser_time; tmp_offset1 += diff_time / count; } catch (hokuyo::RepeatedTimeException &e) { // We expect to get Repeated Time's when hammering on the time server continue; } } uint64_t start_time = timeHelper(); usleep(5000000); sendCmd("TM1;a",timeout); sendCmd("TM1;b",timeout); comp_time = timeHelper(); drift_time = comp_time - start_time; laser_time = readTime() + tmp_offset1; diff_time = comp_time - laser_time; double drift_rate = double(diff_time) / double(drift_time); sendCmd("TM2",timeout); if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0) HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation"); hokuyo::LaserScan scan; count = 200; for (int i = 0; i < count;i++) { try { serviceScan(scan, 1000); } catch (hokuyo::CorruptedDataException &e) { continue; } comp_time = scan.system_time_stamp; drift_time = comp_time - start_time; laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate); diff_time = laser_time - comp_time; tmp_offset2 += diff_time / count; } offset_ = tmp_offset2; stopScanning(); ROS_DEBUG("Leaving calcLatency."); return offset_; }
// returns true, iff node could be added to the cloud bool GraphManager::addNode(Node new_node) { std::clock_t starttime=std::clock(); if(reset_request_) { int numLevels = 3; int nodeDistance = 2; marker_id =0; time_of_last_transform_= ros::Time(); last_batch_update_=std::clock(); delete optimizer_; optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance); graph_.clear(); matches_.clear(); freshlyOptimized_= false; reset_request_ = false; } if (new_node.feature_locations_2d_.size() <= 5) { ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size()); return false; } //set the node id only if the node is actually added to the graph //needs to be done here as the graph size can change inside this function new_node.id_ = graph_.size(); //First Node, so only build its index, insert into storage and add a //vertex at the origin, of which the position is very certain if (graph_.size()==0) { new_node.buildFlannIndex(); // create index so that next nodes can use it // graph_.insert(make_pair(new_node.id_, new_node)); //store graph_[new_node.id_] = new_node; optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin return true; } unsigned int num_edges_before = optimizer_->edges().size(); std::vector<cv::DMatch> initial_matches; ROS_DEBUG("Graphsize: %d", (int) graph_.size()); marker_id = 0; //overdraw old markers Eigen::Matrix4f ransac_trafo, final_trafo; bool edge_added_to_base; std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad int id_of_id = vertices_to_comp.size() -1; for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized. initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo); //initial_matches = processNodePair(new_node, graph_rit->second); //What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++); } id_of_id++;//set back to last valid id if (optimizer_->edges().size() > num_edges_before) { new_node.buildFlannIndex(); graph_[new_node.id_] = new_node; ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size()); optimizeGraph(); //make the transform of the last node known ros::TimerEvent unused; broadcastTransform(unused); visualizeGraphEdges(); visualizeGraphNodes(); visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++); } else { ROS_WARN("##### could not find link for PointCloud!"); matches_.clear(); } ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec"); return (optimizer_->edges().size() > num_edges_before); }
void GraphManager::visualizeFeatureFlow3D(const Node& earlier_node, const Node& newer_node, const std::vector<cv::DMatch>& all_matches, const std::vector<cv::DMatch>& inlier_matches, unsigned int marker_id, bool draw_outlier) const { std::clock_t starttime=std::clock(); if (ransac_marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking visualization_msgs::Marker marker_lines; marker_lines.header.frame_id = "/openni_rgb_optical_frame"; marker_lines.ns = "ransac_markers"; marker_lines.header.stamp = ros::Time::now(); marker_lines.action = visualization_msgs::Marker::ADD; marker_lines.pose.orientation.w = 1.0; marker_lines.id = marker_id; marker_lines.type = visualization_msgs::Marker::LINE_LIST; marker_lines.scale.x = 0.002; std_msgs::ColorRGBA color_red ; //red outlier color_red.r = 1.0; color_red.a = 1.0; std_msgs::ColorRGBA color_green; //green inlier, newer endpoint color_green.g = 1.0; color_green.a = 1.0; std_msgs::ColorRGBA color_yellow; //yellow inlier, earlier endpoint color_yellow.r = 1.0; color_yellow.g = 1.0; color_yellow.a = 1.0; std_msgs::ColorRGBA color_blue ; //red-blue outlier color_blue.b = 1.0; color_blue.a = 1.0; marker_lines.color = color_green; //just to set the alpha channel to non-zero AISNavigation::PoseGraph3D::Vertex* earlier_v; //used to get the transform AISNavigation::PoseGraph3D::Vertex* newer_v; //used to get the transform AISNavigation::PoseGraph3D::VertexIDMap v_idmap = optimizer_->vertices(); // end of initialization ROS_DEBUG("Matches Visualization start"); // write all inital matches to the line_list marker_lines.points.clear();//necessary? if (draw_outlier) { for (unsigned int i=0; i<all_matches.size(); i++) { int newer_id = all_matches.at(i).queryIdx; //feature id in newer node int earlier_id = all_matches.at(i).trainIdx; //feature id in earlier node earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]); newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]); //Outliers are red (newer) to blue (older) marker_lines.colors.push_back(color_red); marker_lines.colors.push_back(color_blue); marker_lines.points.push_back( pointInWorldFrame(newer_node.feature_locations_3d_[newer_id], newer_v->transformation)); marker_lines.points.push_back( pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id], earlier_v->transformation)); } } for (unsigned int i=0; i<inlier_matches.size(); i++) { int newer_id = inlier_matches.at(i).queryIdx; //feature id in newer node int earlier_id = inlier_matches.at(i).trainIdx; //feature id in earlier node earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]); newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]); //inliers are green (newer) to blue (older) marker_lines.colors.push_back(color_green); marker_lines.colors.push_back(color_blue); marker_lines.points.push_back( pointInWorldFrame(newer_node.feature_locations_3d_[newer_id], newer_v->transformation)); marker_lines.points.push_back( pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id], earlier_v->transformation)); } ransac_marker_pub_.publish(marker_lines); ROS_DEBUG_STREAM("Published " << marker_lines.points.size()/2 << " lines"); } ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec"); }
bool ControllerManager::unloadController(const std::string &name) { ROS_DEBUG("Will unload controller '%s'", name.c_str()); // lock the controllers boost::recursive_mutex::scoped_lock guard(controllers_lock_); // get reference to controller list int free_controllers_list = (current_controllers_list_ + 1) % 2; while (ros::ok() && free_controllers_list == used_by_realtime_){ if (!ros::ok()) return false; usleep(200); } std::vector<ControllerSpec> &from = controllers_lists_[current_controllers_list_], &to = controllers_lists_[free_controllers_list]; to.clear(); // Transfers the running controllers over, skipping the one to be removed and the running ones. bool removed = false; for (size_t i = 0; i < from.size(); ++i) { if (from[i].info.name == name){ if (from[i].c->isRunning()){ to.clear(); ROS_ERROR("Could not unload controller with name %s because it is still running", name.c_str()); return false; } removed = true; } else to.push_back(from[i]); } // Fails if we could not remove the controllers if (!removed) { to.clear(); ROS_ERROR("Could not unload controller with name %s because no controller with this name exists", name.c_str()); return false; } // Destroys the old controllers list when the realtime thread is finished with it. ROS_DEBUG("Realtime switches over to new controller list"); int former_current_controllers_list_ = current_controllers_list_; current_controllers_list_ = free_controllers_list; while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){ if (!ros::ok()) return false; usleep(200); } ROS_DEBUG("Destruct controller"); from.clear(); ROS_DEBUG("Destruct controller finished"); ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str()); return true; }
bool ControllerManager::switchController(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers, int strictness) { if (!stop_request_.empty() || !start_request_.empty()) ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen."); if (strictness == 0){ ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.", controller_manager_msgs::SwitchController::Request::STRICT, controller_manager_msgs::SwitchController::Request::BEST_EFFORT); strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT; } ROS_DEBUG("switching controllers:"); for (unsigned int i=0; i<start_controllers.size(); i++) ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str()); for (unsigned int i=0; i<stop_controllers.size(); i++) ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str()); // lock controllers boost::recursive_mutex::scoped_lock guard(controllers_lock_); controller_interface::ControllerBase* ct; // list all controllers to stop for (unsigned int i=0; i<stop_controllers.size(); i++) { ct = getControllerByName(stop_controllers[i]); if (ct == NULL){ if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){ ROS_ERROR("Could not stop controller with name %s because no controller with this name exists", stop_controllers[i].c_str()); stop_request_.clear(); return false; } else{ ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists", stop_controllers[i].c_str()); } } else{ ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers", stop_controllers[i].c_str()); stop_request_.push_back(ct); } } ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size()); // list all controllers to start for (unsigned int i=0; i<start_controllers.size(); i++) { ct = getControllerByName(start_controllers[i]); if (ct == NULL){ if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){ ROS_ERROR("Could not start controller with name %s because no controller with this name exists", start_controllers[i].c_str()); stop_request_.clear(); start_request_.clear(); return false; } else{ ROS_DEBUG("Could not start controller with name %s because no controller with this name exists", start_controllers[i].c_str()); } } else{ ROS_DEBUG("Found controller %s that needs to be started in list of controllers", start_controllers[i].c_str()); start_request_.push_back(ct); } } ROS_DEBUG("Start request vector has size %i", (int)start_request_.size()); // Do the resource management checking std::list<hardware_interface::ControllerInfo> info_list; std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_]; for (size_t i = 0; i < controllers.size(); ++i) { bool in_stop_list = false; for(size_t j = 0; j < stop_request_.size(); j++) in_stop_list = in_stop_list || (stop_request_[j] == controllers[i].c.get()); bool in_start_list = false; for(size_t j = 0; j < start_request_.size(); j++) in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get()); bool add_to_list = controllers[i].c->isRunning(); if (in_stop_list) add_to_list = false; if (in_start_list) add_to_list = true; if (add_to_list) info_list.push_back(controllers[i].info); } bool in_conflict = robot_hw_->checkForConflict(info_list); if (in_conflict) { ROS_ERROR("Could not switch controllers, due to resource conflict"); stop_request_.clear(); start_request_.clear(); return false; } // start the atomic controller switching switch_strictness_ = strictness; please_switch_ = true; // wait until switch is finished ROS_DEBUG("Request atomic controller switch from realtime loop"); while (ros::ok() && please_switch_){ if (!ros::ok()) return false; usleep(100); } start_request_.clear(); stop_request_.clear(); ROS_DEBUG("Successfully switched controllers"); return true; }
bool ControllerManager::loadController(const std::string& name) { ROS_DEBUG("Will load controller '%s'", name.c_str()); // lock controllers boost::recursive_mutex::scoped_lock guard(controllers_lock_); // get reference to controller list int free_controllers_list = (current_controllers_list_ + 1) % 2; while (ros::ok() && free_controllers_list == used_by_realtime_){ if (!ros::ok()) return false; usleep(200); } std::vector<ControllerSpec> &from = controllers_lists_[current_controllers_list_], &to = controllers_lists_[free_controllers_list]; to.clear(); // Copy all controllers from the 'from' list to the 'to' list for (size_t i = 0; i < from.size(); ++i) to.push_back(from[i]); // Checks that we're not duplicating controllers for (size_t j = 0; j < to.size(); ++j) { if (to[j].info.name == name) { to.clear(); ROS_ERROR("A controller named '%s' was already loaded inside the controller manager", name.c_str()); return false; } } ros::NodeHandle c_nh; // Constructs the controller try{ c_nh = ros::NodeHandle(root_nh_, name); } catch(std::exception &e) { ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what()); return false; } catch(...){ ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name.c_str()); return false; } boost::shared_ptr<controller_interface::ControllerBase> c; std::string type; if (c_nh.getParam("type", type)) { ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str()); try { // Trying loading the controller using all of our controller loaders. Exit once we've found the first valid loaded controller std::list<LoaderPtr>::iterator it = controller_loaders_.begin(); while (!c && it != controller_loaders_.end()) { std::vector<std::string> cur_types = (*it)->getDeclaredClasses(); for(size_t i=0; i < cur_types.size(); i++){ if (type == cur_types[i]){ c = (*it)->createInstance(type); } } ++it; } } catch (const std::runtime_error &ex) { ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what()); } } else { ROS_ERROR("Could not load controller '%s' because the type was not specified. Did you load the controller configuration on the parameter server?", name.c_str()); to.clear(); return false; } // checks if controller was constructed if (!c) { ROS_ERROR("Could not load controller '%s' because controller type '%s' does not exist.", name.c_str(), type.c_str()); ROS_ERROR("Use 'rosservice call controller_manager/list_controller_types' to get the available types"); to.clear(); return false; } // Initializes the controller ROS_DEBUG("Initializing controller '%s'", name.c_str()); bool initialized; std::set<std::string> claimed_resources; // Gets populated during initRequest call try{ initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources); } catch(std::exception &e){ ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what()); initialized = false; } catch(...){ ROS_ERROR("Exception thrown while initializing controller %s", name.c_str()); initialized = false; } if (!initialized) { to.clear(); ROS_ERROR("Initializing controller '%s' failed", name.c_str()); return false; } ROS_DEBUG("Initialized controller '%s' succesful", name.c_str()); // Adds the controller to the new list to.resize(to.size() + 1); to[to.size()-1].info.type = type; to[to.size()-1].info.hardware_interface = c->getHardwareInterfaceType(); to[to.size()-1].info.name = name; to[to.size()-1].info.resources = claimed_resources; to[to.size()-1].c = c; // Destroys the old controllers list when the realtime thread is finished with it. int former_current_controllers_list_ = current_controllers_list_; current_controllers_list_ = free_controllers_list; while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){ if (!ros::ok()) return false; usleep(200); } from.clear(); ROS_DEBUG("Successfully load controller '%s'", name.c_str()); return true; }
/// \brief callback for setting models joints states bool setModelsJointsStates(pr2_gazebo_plugins::SetModelsJointsStates::Request &req, pr2_gazebo_plugins::SetModelsJointsStates::Response &res) { ROS_DEBUG("setModelsJointsStates"); return true; }
void GazeboRosControllerManager::ReadPr2Xml() { std::string urdf_param_name; std::string urdf_string; // search and wait for robot_description on param server while(urdf_string.empty()) { ROS_DEBUG("gazebo controller manager plugin is waiting for urdf: %s on the param server.", this->robotParam.c_str()); if (this->rosnode_->searchParam(this->robotParam,urdf_param_name)) { this->rosnode_->getParam(urdf_param_name,urdf_string); ROS_DEBUG("found upstream\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str()); } else { this->rosnode_->getParam(this->robotParam,urdf_string); ROS_DEBUG("found in node namespace\n%s\n------\n%s\n------\n%s",this->robotParam.c_str(),urdf_param_name.c_str(),urdf_string.c_str()); } usleep(100000); } ROS_DEBUG("gazebo controller manager got pr2.xml from param server, parsing it..."); // initialize TiXmlDocument doc with a string TiXmlDocument doc; if (!doc.Parse(urdf_string.c_str()) && doc.Error()) { ROS_ERROR("Could not load the gazebo controller manager plugin's configuration file: %s\n", urdf_string.c_str()); } else { //doc.Print(); //std::cout << *(doc.RootElement()) << std::endl; // Pulls out the list of actuators used in the robot configuration. struct GetActuators : public TiXmlVisitor { std::set<std::string> actuators; virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *) { if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name")) actuators.insert(elt.Attribute("name")); else if (elt.ValueStr() == std::string("rightActuator") && elt.Attribute("name")) actuators.insert(elt.Attribute("name")); else if (elt.ValueStr() == std::string("leftActuator") && elt.Attribute("name")) actuators.insert(elt.Attribute("name")); return true; } } get_actuators; doc.RootElement()->Accept(&get_actuators); // Places the found actuators into the hardware interface. std::set<std::string>::iterator it; for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it) { //std::cout << " adding actuator " << (*it) << std::endl; pr2_hardware_interface::Actuator* pr2_actuator = new pr2_hardware_interface::Actuator(*it); pr2_actuator->state_.is_enabled_ = true; this->hw_.addActuator(pr2_actuator); } // Setup mechanism control node this->cm_->initXml(doc.RootElement()); for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i) this->cm_->state_->joint_states_[i].calibrated_ = fake_calibration_; } }
void DecosSensorRing::ReadSensorRingValues() { int r = 1; if(fd == P_ERROR) { ROS_ERROR("Port not opened. Abort!"); return; } int totalrx = readData(fd, rxbuf, 64); rxbuf[totalrx] = 0; //ROS_INFO("RX: <%s>", rxbuf); for (int i=0; i<totalrx; i++) { unsigned char z = rxbuf[i]; // one frame = [I|%03i|%03i|%03i|%03i|%03i|%03i|U|%04i] switch (rxstate) // this is our state machine to read one full frame of values { case 0: if (z=='[') rxstate++; break; case 1: if (z=='I') rxstate++; else rxstate = 0; break; case 2: case 4: case 6: case 8: case 10: case 12: case 14: case 16: if (z=='|') { rxstate++; rxcount = 0; } else rxstate = 0; break; case 3: case 5: case 7: case 9: case 11: case 13: if (CollectValue(z, 3)==0) // done with one value { int idx = (int)(rxstate-3)/2; if ((idx>=0) && (idx<6)) sensorSet.IR[idx] = atoi(collectedvalue); } break; case 15: if (z=='U') rxstate++; else rxstate = 0; break; case 17: if (CollectValue(z, 4)==0) // got value? { sensorSet.US = atoi(collectedvalue); sensorSet.counter++; sensorSet.valid = true; rxstate = 0; } break; case 18: // in case a fail occured at step 17 in CollectValue rxstate = 0; break; } } if (sensorSet.valid) { ROS_DEBUG("Set: %03i, %03i, %03i, %03i, %03i, %03i | %04i", sensorSet.IR[0], sensorSet.IR[1], sensorSet.IR[2], sensorSet.IR[3], sensorSet.IR[4], sensorSet.IR[5], sensorSet.US); sensorSet.valid = false; decos_sensorring::SensorValues sv; sv.IR1 = sensorSet.IR[0]; sv.IR2 = sensorSet.IR[1]; sv.IR3 = sensorSet.IR[2]; sv.IR4 = sensorSet.IR[3]; sv.IR5 = sensorSet.IR[4]; sv.IR6 = sensorSet.IR[5]; sv.US = sensorSet.US; sensorPub.publish(sv); } }
//returns a single point cloud colored objects , void CObjectCandidateExtraction::extractObjectCandidates(pcl::PointCloud< pcl::PointXYZRGB> &point_cloud, pcl::PointCloud<pcl::PointXYZRGBNormal> &planar_point_cloud, std::vector<structPlanarSurface> &hierarchyPlanes) { ROS_DEBUG("[extractObjectCandidates] extractObjectCandidates started ..."); ros::Time start, start2, finish, finish2, start3, finish3; start = ros::Time::now(); std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredObjects; std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> > clusteredPlanes; pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_RGB; pcl::PointIndices inliers, inliersObjects; pcl::PointCloud<pcl::PointXYZRGBNormal> total_point_cloud, point_cloud_normal; //std::vector<structPlanarSurface> hierarchyPlanes; // std::vector<bool> delete_total_point_cloud; ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares started ... "); // total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares( // point_cloud, 0.02f); //0.02f works good with 0.008 subsampling//0.01 veryfast but not good total_point_cloud = point_cloud_normal = this->toolBox.movingLeastSquares( point_cloud, 0.01f); //0.02f works good //0.01 veryfast but not good ROS_DEBUG("[extractObjectCandidates] MovingLeastSquares done ... "); hierarchyPlanes = horizontalSurfaceExtractor.extractMultiplePlanes( point_cloud_normal, planar_point_cloud, clusteredPlanes, 2); if (hierarchyPlanes.empty()) { return; } for (unsigned int indexMaxPointsClusteredPlane = 0; indexMaxPointsClusteredPlane < hierarchyPlanes.size(); indexMaxPointsClusteredPlane++) { //planar_point_cloud = hierarchyPlanes[indexMaxPointsClusteredPlane].pointCloud; //--------------------------------------------- // get plane hull start2 = ros::Time::now(); pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_hull; // pcl::ConvexHull2D<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> chull; // chull.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZRGBNormal> > (planar_point_cloud)); // chull.reconstruct (cloud_hull); cloud_hull = hierarchyPlanes[indexMaxPointsClusteredPlane].convexHull; //pointCloud; //ROS_DEBUG("[%s/extractObjectCandidates] dZmax %f dZmin %f",this->nodeName.c_str(),dZmax,dZmin ); pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_objects; cloud_objects.header = total_point_cloud.header; bool reject = false; unsigned int total_point_cloud_size = total_point_cloud.points.size(); // int chunk = total_point_cloud_size / 4; omp_set_num_threads(4); #pragma omp parallel shared (total_point_cloud,chunk) private(j) { #pragma omp for schedule(dynamic,chunk) nowait for (unsigned int j = 0; j < total_point_cloud_size; ++j) { //here check also whether point below upper plane is if (toolBox.pointInsideConvexHull2d(cloud_hull, total_point_cloud.points[j]) && total_point_cloud.points[j].z > (toolBox.getNearestNeighborPlane( hierarchyPlanes[indexMaxPointsClusteredPlane], total_point_cloud.points[j]).z + this->threshold_point_above_lower_plane)) //(dZmax + this->threshold_point_above_lower_plane))//(dZmax+this->threshold_point_above_lower_plane)) //dZmax //dZmax-(dZmax*0.005)) //(((dZmax+dZmin)/2)+dZmax)/2) { reject = false; for (unsigned int iterUpperPlanes = 0; iterUpperPlanes < hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces.size(); iterUpperPlanes++) { /* if (toolBox.pointInsideConvexHull2d( hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].convexHull, total_point_cloud.points[j]) && total_point_cloud.points[j].z > hierarchyPlanes[indexMaxPointsClusteredPlane].upperPlanarSurfaces[iterUpperPlanes].plane_height + THRESHOLD_POINT_ABOVE_UPPER_PLANE) //a little bit over surface(upperplane) in order to get a cube as a cube and not without uppersurface { reject = true; break; }*/ } if (!reject) { cloud_objects.points.push_back( total_point_cloud.points[j]); } } } } //PRAGMA cloud_objects.width = cloud_objects.points.size(); cloud_objects.height = 1; //--------------------------------------------- ROS_DEBUG ("[extractObjectCandidates] Number of object point candidates: %d.", (int)cloud_objects.points.size()); if ((unsigned int) cloud_objects.points.size() > 0) { //Cluster objects pcl::EuclideanClusterExtraction<pcl::PointXYZRGBNormal> euclideanClusterExtractor; std::vector<pcl::PointIndices> clusteredObjectIndices; euclideanClusterExtractor.setInputCloud(cloud_objects.makeShared()); euclideanClusterExtractor.setClusterTolerance(0.025); //(0.02); euclideanClusterExtractor.setMinClusterSize(30); euclideanClusterExtractor.extract(clusteredObjectIndices); ROS_DEBUG ("[extractObjectCandidates] Number of objects clustered: %d",(int)clusteredObjectIndices.size()); ///clusteredObjects.resize((int)clusteredObjectIndices.size()); for (unsigned int iterCluster = 0; iterCluster < clusteredObjectIndices.size(); iterCluster++) { pcl::PointCloud<pcl::PointXYZRGBNormal> foundObject; pcl::PointCloud<pcl::PointXYZRGBNormal> foundObjectFull; pcl::PointCloud<pcl::PointXYZRGBNormal> foundObjectHull; //pcl::copyPointCloud(cloud_objects,clusteredObjectIndices.at(iterCluster),clusteredObjects.at(iterCluster) ); pcl::copyPointCloud(cloud_objects, clusteredObjectIndices.at( iterCluster), foundObject); if (!toolBox.isObjectPlane( hierarchyPlanes[indexMaxPointsClusteredPlane], foundObject, IS_PLANE_OBJECT__OBJECT_HEIGHT_THRESHOLD, IS_PLANE_OBJECT__OBJECT_PLANE_HEIGHT_DIFFERENCE) && foundObject.points.size()>this->min_points_per_objects) { ROS_DEBUG("[extractObjectCandidates] Object(%d) added",iterCluster); /* pcl::ConvexHull2D<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> convexHullExtractor; convexHullExtractor.setInputCloud(boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBNormal> >( foundObject)); convexHullExtractor.reconstruct(foundObjectHull); toolBox.get3DPointsWithinHull(total_point_cloud, foundObjectHull, 0, toolBox.maxValuePointCloud3d(foundObject, 2)-toolBox.minValuePointCloud3d(foundObject, 2), foundObjectFull); */ foundObjectFull = foundObject; hierarchyPlanes[indexMaxPointsClusteredPlane].clusteredObjects.push_back( foundObjectFull); } } //------------------------------------------------------------------------------------------------- //add potential related points to objects (below the object and plane), how it is only interesting if we use it for rgb classfication //pcl::KdTreeANN<pcl::PointXYZRGB>::Ptr objectTree = boost::make_shared<pcl::KdTreeANN<pcl::PointXYZRGB> > (); //objectTree->setInputCloud(total_point_cloud); /* for(unsigned int j=0; j < clusteredObjects.size(); ++j) { double dXObjectmin = 90, dYObjectmin = 90, dXObjectmax = -90, dYObjectmax=-90, dZObjectmax=-90, dZObjectmin=90; double dObjectHeight; for(unsigned int i=0; i < clusteredObjects[j].points.size(); ++i) { if(clusteredObjects[j].points[i].x < dXObjectmin) dXObjectmin = clusteredObjects[j].points[i].x; if(clusteredObjects[j].points[i].x > dXObjectmax) dXObjectmax = clusteredObjects[j].points[i].x; if(clusteredObjects[j].points[i].y < dYObjectmin) dYObjectmin = clusteredObjects[j].points[i].y; if(clusteredObjects[j].points[i].y > dYObjectmax) dYObjectmax = clusteredObjects[j].points[i].y; if(clusteredObjects[j].points[i].z < dZObjectmin) dZObjectmin = clusteredObjects[j].points[i].z; if(clusteredObjects[j].points[i].z > dZObjectmax) dZObjectmax = clusteredObjects[j].points[i].z; } dObjectHeight = dZObjectmax - dZObjectmin; ROS_INFO ("objects no %d -> points %d", j, (int)clusteredObjects[j].points.size()); bool found=false; for(unsigned int k=0; k < total_point_cloud.points.size(); ++k) { if(total_point_cloud.points[k].x >= dXObjectmin && total_point_cloud.points[k].x <= dXObjectmax && total_point_cloud.points[k].y >= dYObjectmin && total_point_cloud.points[k].y <= dYObjectmax && total_point_cloud.points[k].z >= (dZmax)) //dZmin { //check if point already in point cloud for(unsigned int l=0;l < clusteredObjects[j].points.size(); ++l ) { if(clusteredObjects[j].points[l].x==total_point_cloud.points[k].x && clusteredObjects[j].points[l].y==total_point_cloud.points[k].y && clusteredObjects[j].points[l].z==total_point_cloud.points[k].z) { found=true; break; } } if(found==false) clusteredObjects[j].points.push_back(total_point_cloud.points[k]); found=false; } } ROS_INFO ("objects no %d -> points %d", j, (int)clusteredObjects[j].points.size()); } */ //------------------ //TIP:pcl->delete tabletop-> get each object pcl-> do distance intensity -> perfect object training without any texture (project object on black background so we can apply 2d/3d stuff) // : then take several view, do the same, do ICP/toro or merge all points to get a 3d model for viewpoint estimation e.g. // : match points from query with icp to find best fit. but we need to know before hand what kind of object we found-> so apply recognition/classification beforehand //// } finish2 = ros::Time::now(); ROS_DEBUG("[extractObjectCandidates] %d. plane computed - (%lf)",indexMaxPointsClusteredPlane ,(finish2.toSec() - start2.toSec() )); } //clusteredObjectsInput = clusteredObjects; finish = ros::Time::now(); ROS_DEBUG("[extractObjectCandidates] Execution time = %lf",(finish.toSec() - start.toSec() )); }
/** Dynamic reconfigure callback * * Called immediately when callback first defined. Called again * when dynamic reconfigure starts or changes a parameter value. * * @param newconfig new Config values * @param level bit-wise OR of reconfiguration levels for all * changed parameters (0xffffffff on initial call) **/ void Camera1394Driver::reconfig(Config &newconfig, uint32_t level) { // Do not run concurrently with poll(). Tell it to stop running, // and wait on the lock until it does. reconfiguring_ = true; boost::mutex::scoped_lock lock(mutex_); ROS_DEBUG("dynamic reconfigure level 0x%x", level); // resolve frame ID using tf_prefix parameter if (newconfig.frame_id == "") newconfig.frame_id = "camera"; std::string tf_prefix = tf::getPrefixParam(priv_nh_); ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix); newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id); if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE)) { // must close the device before updating these parameters closeCamera(); // state_ --> CLOSED } if (state_ == Driver::CLOSED) { // open with new values openCamera(newconfig); } if (config_.camera_info_url != newconfig.camera_info_url) { if (!validateConfig(newconfig)) { // new URL not valid, use the old one newconfig.camera_info_url = config_.camera_info_url; } } if (state_ != Driver::CLOSED) // openCamera() succeeded? { // configure IIDC features if (level & Levels::RECONFIGURE_CLOSE) { // initialize all features for newly opened device if (false == dev_->features_->initialize(&newconfig)) { ROS_ERROR_STREAM("[" << camera_name_ << "] feature initialization failure"); closeCamera(); // can't continue } } else { // update any features that changed // TODO replace this with a dev_->reconfigure(&newconfig); dev_->features_->reconfigure(&newconfig); } } config_ = newconfig; // save new parameters reconfiguring_ = false; // let poll() run again ROS_DEBUG_STREAM("[" << camera_name_ << "] reconfigured: frame_id " << newconfig.frame_id << ", camera_info_url " << newconfig.camera_info_url); }
void Commander::keyLoop() { char c; // get the console in raw mode tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); // Setting a new line, then end of file raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); puts("Reading from keyboard"); puts("---------------------------"); puts("Use arrow keys to move the ROBOT."); for(;;) { // get the next event from the keyboard LunarNXT::Order msg; if(read(kfd, &c, 1) < 0) { perror("read():"); exit(-1); } ROS_DEBUG("value: 0x%02X\n", c); switch(c) { case KEYCODE_Q: ROS_INFO("Turn Left !"); msg.sOrder = "turn_l"; break; case KEYCODE_D: ROS_INFO("Turn Right !"); msg.sOrder = "turn_r"; break; case KEYCODE_Z: ROS_INFO("Go !"); msg.sOrder = "go"; break; case KEYCODE_S: ROS_INFO("Back !"); msg.sOrder = "back"; break; case KEYCODE_A: ROS_INFO("Stop !"); msg.sOrder = "stop"; break; case KEYCODE_L: ROS_INFO("LineLauncher"); msg.sOrder = "launch"; break; case KEYCODE_M: ROS_INFO("LineUnLauncher"); msg.sOrder = "unlaunch"; break; } this->commander_pub.publish(msg); } return; }
bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, grid_map::GridMap& gridMap, const double lowerValue, const double upperValue) { cv_bridge::CvImagePtr cvPtrAlpha, cvPtrMono; // If alpha channel exist, read it. if (image.encoding == sensor_msgs::image_encodings::BGRA8 || image.encoding == sensor_msgs::image_encodings::BGRA16) { try { cvPtrAlpha = cv_bridge::toCvCopy(image, image.encoding); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return false; } } unsigned int depth; // Convert color image to grayscale. try { if (image.encoding == sensor_msgs::image_encodings::BGRA8 || image.encoding == sensor_msgs::image_encodings::BGR8 || image.encoding == sensor_msgs::image_encodings::MONO8) { cvPtrMono = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::MONO8); depth = std::pow(2, 8); ROS_DEBUG("Color image converted to mono8"); } else if (image.encoding == sensor_msgs::image_encodings::BGRA16 || image.encoding == sensor_msgs::image_encodings::BGR16 || image.encoding == sensor_msgs::image_encodings::MONO16) { cvPtrMono = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::MONO16); depth = std::pow(2, 16); ROS_DEBUG("Color image converted to mono16"); } else { ROS_ERROR("Expected BGR, BGRA, or MONO image encoding."); return false; } } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return false; } gridMap.add(layer); if (gridMap.getSize()(0) != image.height || gridMap.getSize()(1) != image.width) { ROS_ERROR("Image size does not correspond to grid map size!"); return false; } for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { // Set transparent values. if (image.encoding == sensor_msgs::image_encodings::BGRA8) { const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0), (*iterator)(1)); unsigned int alpha = cvAlpha[3]; if (cvAlpha[3] < depth / 2) continue; } if (image.encoding == sensor_msgs::image_encodings::BGRA16) { const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec<uchar, 8>>( (*iterator)(0), (*iterator)(1)); int alpha = (cvAlpha[6] << 8) + cvAlpha[7]; if (alpha < depth / 2) continue; } // Compute height. unsigned int grayValue; if (depth == std::pow(2, 8)) { uchar cvGrayscale = cvPtrMono->image.at<uchar>((*iterator)(0), (*iterator)(1)); grayValue = cvGrayscale; } if (depth == std::pow(2, 16)) { const auto& cvGrayscale = cvPtrMono->image.at<cv::Vec2b>((*iterator)(0), (*iterator)(1)); grayValue = (cvGrayscale[0] << 8) + cvGrayscale[1]; } double height = lowerValue + (upperValue - lowerValue) * ((double) grayValue / (double) (depth - 1)); gridMap.at(layer, *iterator) = height; } return true; }
void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg) { ROS_DEBUG("HectorCM: received new elevation map"); // check header if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) && // Magic number 1000 -> min grid size should not be less than 1mm elevation_map_msg->info.height != cost_map.info.height && elevation_map_msg->info.width != cost_map.info.width) { ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!"); return; } // store elevation_map_msg in local variable elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width); // store elevation map zero level elevation_zero_level = elevation_map_msg->info.zero_elevation; // compute region of intereset if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world)) return; // loop through each element int filtered_cell, filtered_cell_x, filtered_cell_y; for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { // compute cost_map_index unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v); // check if neighbouring cells are known if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) || elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level)) continue; // edge filter filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1)); filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u)); if(filtered_cell_x > filtered_cell_y) filtered_cell = filtered_cell_x; else filtered_cell = filtered_cell_y; // check if cell is traversable if(filtered_cell > max_delta_elevation/grid_res_z) { // cell is not traversable -> mark it as occupied elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL; } else { // cell is traversable -> mark it as free elevation_cost_map.data[cost_map_index] = FREE_CELL; } } } // set elevation map received flag received_elevation_map = true; // calculate cost map if(received_grid_map) { if(received_point_cloud) { calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP); } else { calculateCostMap(USE_GRID_AND_ELEVATION_MAP); } } else { calculateCostMap(USE_ELEVATION_MAP_ONLY); } }
int HandymanTeleopKey::run(int argc, char **argv) { char c; ///////////////////////////////////////////// // get the console in raw mode int kfd = 0; struct termios cooked; struct termios raw; tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); ///////////////////////////////////////////// showHelp(); ros::init(argc, argv, "handyman_teleop_key"); ros::NodeHandle node_handle; // Override the default ros sigint handler. // This must be set after the first NodeHandle is created. signal(SIGINT, rosSigintHandler); ros::Rate loop_rate(40); std::string sub_msg_to_robot_topic_name; std::string pub_msg_to_moderator_topic_name; std::string sub_joint_state_topic_name; std::string pub_base_twist_topic_name; std::string pub_arm_trajectory_topic_name; std::string pub_gripper_trajectory_topic_name; node_handle.param<std::string>("sub_msg_to_robot_topic_name", sub_msg_to_robot_topic_name, "/handyman/message/to_robot"); node_handle.param<std::string>("pub_msg_to_moderator_topic_name", pub_msg_to_moderator_topic_name, "/handyman/message/to_moderator"); node_handle.param<std::string>("sub_joint_state_topic_name", sub_joint_state_topic_name, "/hsrb/joint_states"); node_handle.param<std::string>("pub_base_twist_topic_name", pub_base_twist_topic_name, "/hsrb/opt_command_velocity"); node_handle.param<std::string>("pub_arm_trajectory_topic_name", pub_arm_trajectory_topic_name, "/hsrb/arm_trajectory_controller/command"); node_handle.param<std::string>("pub_gripper_trajectory_topic_name", pub_gripper_trajectory_topic_name, "/hsrb/gripper_trajectory_controller/command"); ros::Subscriber sub_msg = node_handle.subscribe<handyman::HandymanMsg>(sub_msg_to_robot_topic_name, 100, &HandymanTeleopKey::messageCallback, this); ros::Publisher pub_msg = node_handle.advertise<handyman::HandymanMsg>(pub_msg_to_moderator_topic_name, 10); ros::Subscriber sub_joint_state = node_handle.subscribe<sensor_msgs::JointState>(sub_joint_state_topic_name, 10, &HandymanTeleopKey::jointStateCallback, this); ros::Publisher pub_base_twist = node_handle.advertise<geometry_msgs::Twist> (pub_base_twist_topic_name, 10); ros::Publisher pub_arm_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_arm_trajectory_topic_name, 10); ros::Publisher pub_gripper_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_gripper_trajectory_topic_name, 10); const float linear_coef = 0.2f; const float linear_oblique_coef = 0.141f; const float angular_coef = 0.5f; float move_speed = 1.0f; bool is_hand_open = false; std::string arm_lift_joint_name = "arm_lift_joint"; std::string arm_flex_joint_name = "arm_flex_joint"; std::string wrist_flex_joint_name = "wrist_flex_joint"; while (ros::ok()) { if(canReceive(kfd)) { // get the next event from the keyboard if(read(kfd, &c, 1) < 0) { perror("read():"); exit(EXIT_FAILURE); } switch(c) { case KEYCODE_0: { sendMessage(pub_msg, MSG_I_AM_READY); break; } case KEYCODE_1: { sendMessage(pub_msg, MSG_ROOM_REACHED); break; } case KEYCODE_2: { sendMessage(pub_msg, MSG_OBJECT_GRASPED); break; } case KEYCODE_3: { sendMessage(pub_msg, MSG_TASK_FINISHED); break; } case KEYCODE_6: { sendMessage(pub_msg, MSG_DOES_NOT_EXIST); break; } case KEYCODE_9: { sendMessage(pub_msg, MSG_GIVE_UP); break; } case KEYCODE_UP: { ROS_DEBUG("Go Forward"); moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0); break; } case KEYCODE_DOWN: { ROS_DEBUG("Go Backward"); moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0); break; } case KEYCODE_RIGHT: { ROS_DEBUG("Go Right"); moveBase(pub_base_twist, 0.0, 0.0, -angular_coef*move_speed); break; } case KEYCODE_LEFT: { ROS_DEBUG("Go Left"); moveBase(pub_base_twist, 0.0, 0.0, +angular_coef*move_speed); break; } case KEYCODE_S: { ROS_DEBUG("Stop"); moveBase(pub_base_twist, 0.0, 0.0, 0.0); break; } case KEYCODE_U: { ROS_DEBUG("Move Left Forward"); moveBase(pub_base_twist, +linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0); break; } case KEYCODE_I: { ROS_DEBUG("Move Forward"); moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0); break; } case KEYCODE_O: { ROS_DEBUG("Move Right Forward"); moveBase(pub_base_twist, +linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0); break; } case KEYCODE_J: { ROS_DEBUG("Move Left"); moveBase(pub_base_twist, 0.0, +linear_coef*move_speed, 0.0); break; } case KEYCODE_K: { ROS_DEBUG("Stop"); moveBase(pub_base_twist, 0.0, 0.0, 0.0); break; } case KEYCODE_L: { ROS_DEBUG("Move Right"); moveBase(pub_base_twist, 0.0, -linear_coef*move_speed, 0.0); break; } case KEYCODE_M: { ROS_DEBUG("Move Left Backward"); moveBase(pub_base_twist, -linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0); break; } case KEYCODE_COMMA: { ROS_DEBUG("Move Backward"); moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0); break; } case KEYCODE_PERIOD: { ROS_DEBUG("Move Right Backward"); moveBase(pub_base_twist, -linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0); break; } case KEYCODE_Q: { ROS_DEBUG("Move Speed Up"); move_speed *= 2; if(move_speed > 2 ){ move_speed=2; } break; } case KEYCODE_Z: { ROS_DEBUG("Move Speed Down"); move_speed /= 2; if(move_speed < 0.125){ move_speed=0.125; } break; } case KEYCODE_Y: { ROS_DEBUG("Move Arm Up"); moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.69, std::max<int>((int)(std::abs(0.69 - arm_lift_joint_pos1_) / 0.05), 1)); break; } case KEYCODE_H: { ROS_DEBUG("Stop Arm"); moveArm(pub_arm_trajectory, arm_lift_joint_name, 2.0*arm_lift_joint_pos1_-arm_lift_joint_pos2_, 0.5); break; } case KEYCODE_N: { ROS_DEBUG("Move Arm Down"); moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.0, std::max<int>((int)(std::abs(0.0 - arm_lift_joint_pos1_) / 0.05), 1)); break; } case KEYCODE_A: { ROS_DEBUG("Rotate Arm - Vertical"); moveArm(pub_arm_trajectory, arm_flex_joint_name, 0.0, 1); moveArm(pub_arm_trajectory, wrist_flex_joint_name, -1.57, 1); break; } case KEYCODE_B: { ROS_DEBUG("Rotate Arm - Upward"); moveArm(pub_arm_trajectory, arm_flex_joint_name, -0.785, 1); moveArm(pub_arm_trajectory, wrist_flex_joint_name, -0.785, 1); break; } case KEYCODE_C: { ROS_DEBUG("Rotate Arm - Horizontal"); moveArm(pub_arm_trajectory, arm_flex_joint_name, -1.57, 1); moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.0, 1); break; } case KEYCODE_D: { ROS_DEBUG("Rotate Arm - Downward"); moveArm(pub_arm_trajectory, arm_flex_joint_name, -2.2, 1); moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.35, 1); break; } case KEYCODE_G: { moveHand(pub_gripper_trajectory, is_hand_open); is_hand_open = !is_hand_open; break; } } } ros::spinOnce(); loop_rate.sleep(); } ///////////////////////////////////////////// // cooked mode tcsetattr(kfd, TCSANOW, &cooked); ///////////////////////////////////////////// return EXIT_SUCCESS; }
void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>); ROS_DEBUG("HectorCM: received new point cloud"); // converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data pcl::fromROSMsg(*cloud_msg, *cloud_sensor); // transform cloud to /map frame try { listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_msg->header.stamp,ros::Duration(1.0)); pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); ROS_DEBUG("HectorCM: pointcloud transform failed"); return; } // compute region of intereset if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world)) return; Eigen::Vector2f world_coords; // define a cube with two points in space: Eigen::Vector4f minPoint; world_coords = world_map_transform.getC1Coords(min_index.cast<float>()); minPoint[0]=world_coords(0); // define minimum point x minPoint[1]=world_coords(1); // define minimum point y minPoint[2]=slize_min_height; // define minimum point z Eigen::Vector4f maxPoint; world_coords = world_map_transform.getC1Coords(max_index.cast<float>()); maxPoint[0]=world_coords(0); // define max point x maxPoint[1]=world_coords(1); // define max point y maxPoint[2]=slize_max_height; // define max point z pcl::CropBox<pcl::PointXYZ> cropFilter; cropFilter.setInputCloud (cloud_base_link); cropFilter.setMin(minPoint); cropFilter.setMax(maxPoint); cropFilter.filter (*sliced_cloud); ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size()); pub_cloud_slice.publish(sliced_cloud); cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL); // iterate trough all points for (unsigned int k = 0; k < sliced_cloud->size(); ++k) { const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k]; // allign grid points Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y); Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world)); cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL; } // set point cloud received flag received_point_cloud = true; // calculate cost map if(received_grid_map) { if(received_elevation_map) { calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP); } else { calculateCostMap(USE_GRID_AND_CLOUD_MAP); } } }
/** Dynamic reconfigure callback * * Called immediately when callback first defined. Called again * when dynamic reconfigure starts or changes a parameter value. * * @param newconfig new Config values * @param level bit-wise OR of reconfiguration levels for all * changed parameters (0xffffffff on initial call) **/ void reconfig(Config &newconfig, uint32_t level) { ROS_DEBUG("dynamic reconfigure level 0x%x", level); // resolve frame ID using tf_prefix parameter if (newconfig.frame_id == "") newconfig.frame_id = "camera"; std::string tf_prefix = tf::getPrefixParam(privNH_); ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix); newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id); if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE)) { // must close the device before updating these parameters closeCamera(); // state_ --> CLOSED } if (state_ == Driver::CLOSED) { // open with new values if (openCamera(newconfig)) { // update camera name string newconfig.camera_name = camera_name_; } } ///THIS IS A QUICK HACK TO GET EXPOSURE ON OUR CAMERA'S THIS DOES NOT WORK FOR ALL CAMERAS if(config_.exposure != newconfig.exposure){ try { cam_->set_control(0x9a0901, newconfig.exposure); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting exposure. Exception was " << e.what()); } } if(config_.absolute_exposure != newconfig.absolute_exposure){ try { cam_->set_control(0x9a0902, newconfig.absolute_exposure); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting absolute exposure. Exception was " << e.what()); } } if(config_.sharpness != newconfig.sharpness){ try { cam_->set_control(0x98091b, newconfig.sharpness); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting sharpness. Exception was " << e.what()); } } if(config_.power_line_frequency != newconfig.power_line_frequency){ try { cam_->set_control(0x980918, newconfig.power_line_frequency); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting powerline frequency. Exception was " << e.what()); } } if(config_.white_balance_temperature != newconfig.white_balance_temperature){ try { cam_->set_control(0x98090c, newconfig.white_balance_temperature); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting white balance temperature. Exception was " << e.what()); } } if(config_.gain != newconfig.gain){ try { cam_->set_control(0x980913, newconfig.gain); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting gain. Exception was " << e.what()); } } if(config_.saturation != newconfig.saturation){ try { cam_->set_control(0x980902, newconfig.saturation); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting saturation. Exception was " << e.what()); } } if(config_.contrast != newconfig.contrast){ try { cam_->set_control(0x980901, newconfig.contrast); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting contrast. Exception was " << e.what()); } } if(config_.brightness != newconfig.brightness){ try { cam_->set_control(0x980900, newconfig.brightness); } catch (uvc_cam::Exception& e) { ROS_ERROR_STREAM("Problem setting brightness. Exception was " << e.what()); } } if (config_.camera_info_url != newconfig.camera_info_url) { // set the new URL and load CameraInfo (if any) from it if (cinfo_.validateURL(newconfig.camera_info_url)) { cinfo_.loadCameraInfo(newconfig.camera_info_url); } else { // new URL not valid, use the old one newconfig.camera_info_url = config_.camera_info_url; } } // if (state_ != Driver::CLOSED) // openCamera() succeeded? // { // // configure IIDC features // if (level & Levels::RECONFIGURE_CLOSE) // { // // initialize all features for newly opened device // if (false == dev_->features_->initialize(&newconfig)) // { // ROS_ERROR_STREAM("[" << camera_name_ // << "] feature initialization failure"); // closeCamera(); // can't continue // } // } // else // { // // update any features that changed // dev_->features_->reconfigure(&newconfig); // } // } config_ = newconfig; // save new parameters ROS_DEBUG_STREAM("[" << camera_name_ << "] reconfigured: frame_id " << newconfig.frame_id << ", camera_info_url " << newconfig.camera_info_url); }
bool CostMapCalculation::calculateCostMap_old(char map_level) { switch(map_level) { case USE_GRID_MAP_ONLY: { // cost map based on grid map only ROS_DEBUG("HectorCM: compute costmap based on grid map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if((char)grid_map_.data[index] != UNKNOWN_CELL) { if(grid_map_.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is not occupied cost_map.data[index] = FREE_CELL; } } } } break; } case USE_ELEVATION_MAP_ONLY: { // cost map based on elevation map only ROS_DEBUG("HectorCM: compute costmap based on elevation map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(elevation_cost_map.data[index] != UNKNOWN_CELL) { if(elevation_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is not occupied cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_ELEVATION_MAP: { // cost map based on elevation and grid map ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map"); int checksum_grid_map; // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL) { checksum_grid_map = 0; checksum_grid_map += grid_map_.at<int8_t>(v-1, u); checksum_grid_map += grid_map_.at<int8_t>(v+1, u); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells) { if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL) { // cell is free cost_map.data[index] = FREE_CELL; } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_CLOUD_MAP: { // cost map based on cloud map and grid map ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { cost_map.data[index] = FREE_CELL; } } } } break; } case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP: { // cost map based on elevation, cloud and grid map ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map"); int checksum_grid_map; // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); // check if cell is known if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL) { if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL) { checksum_grid_map = 0; checksum_grid_map += grid_map_.at<int8_t>(v-1, u); checksum_grid_map += grid_map_.at<int8_t>(v+1, u); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells) { if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL) { // cell is free cost_map.data[index] = FREE_CELL; } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } } else { if(cloud_cost_map.data[index] == OCCUPIED_CELL) { // cell is occupied cost_map.data[index] = OCCUPIED_CELL; } else { // cell is free cost_map.data[index] = FREE_CELL; } } } } } break; } } ROS_DEBUG("HectorCM: computed a new costmap"); return true; }
void GSPipe::publish_stream() { ROS_INFO_STREAM("Publishing stream..."); // Pre-roll camera if needed if (preroll_) { ROS_DEBUG("Performing preroll..."); //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll //I am told this is needed and am erring on the side of caution. gst_element_set_state(pipeline_, GST_STATE_PLAYING); if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Failed to PLAY during preroll."); return; } else { ROS_DEBUG("Stream is PLAYING in preroll."); } gst_element_set_state(pipeline_, GST_STATE_PAUSED); if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Failed to PAUSE."); return; } else { ROS_INFO("Stream is PAUSED in preroll."); } } if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { ROS_ERROR("Could not start stream!"); return; } ROS_INFO("Started stream."); // Poll the data as fast a spossible while(ros::ok()) { // This should block until a new frame is awake, this way, we'll run at the // actual capture framerate of the device. ROS_DEBUG("Getting data..."); GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_)); GstFormat fmt = GST_FORMAT_TIME; gint64 current = -1; // Query the current position of the stream //if (gst_element_query_position(pipeline_, &fmt, ¤t)) { //ROS_INFO_STREAM("Position "<<current); //} // Stop on end of stream if (!buf) { ROS_INFO("Stream ended."); break; } ROS_DEBUG("Got data."); // Get the image width and height GstPad* pad = gst_element_get_static_pad(sink_, "sink"); const GstCaps *caps = gst_pad_get_negotiated_caps(pad); GstStructure *structure = gst_caps_get_structure(caps,0); gst_structure_get_int(structure,"width",&width_); gst_structure_get_int(structure,"height",&height_); // Complain if the returned buffer is smaller than we expect const unsigned int expected_frame_size = image_encoding_ == sensor_msgs::image_encodings::RGB8 ? width_ * height_ * 3 : width_ * height_; if (buf->size < expected_frame_size) { ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be " << expected_frame_size << " bytes but got only " << (buf->size) << " bytes. (make sure frames are correctly encoded)"); } // Construct Image message sensor_msgs::ImagePtr img(new sensor_msgs::Image()); sensor_msgs::CameraInfoPtr cinfo; // Update header information cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo())); cinfo->header.stamp = ros::Time::now(); cinfo->header.frame_id = frame_id_; img->header = cinfo->header; // Image data and metadata img->width = width_; img->height = height_; img->encoding = image_encoding_; img->is_bigendian = false; img->data.resize(expected_frame_size); ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_); // Copy only the data we received // Since we're publishing shared pointers, we need to copy the image so // we can free the buffer allocated by gstreamer if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { img->step = width_ * 3; } else { img->step = width_; } std::copy( buf->data, (buf->data)+(buf->size), img->data.begin()); // Publish the image/info camera_pub_.publish(img, cinfo); // Release the buffer gst_buffer_unref(buf); ros::spinOnce(); } }
bool CostMapCalculation::calculateCostMap(char map_level) { if (!map_level) { ROS_WARN("Invalid map selection was given to cost map calculation!"); return false; } if (map_level & GRID_MAP) ROS_DEBUG("HectorCM: compute costmap based on grid map"); if (map_level & ELEVATION_MAP) ROS_DEBUG("HectorCM: compute costmap based on elevation map"); if (map_level & CLOUD_MAP) ROS_DEBUG("HectorCM: compute costmap based on cloud map"); // loop through each element for (int v = min_index(1); v < max_index(1); ++v) { for (int u = min_index(0); u < max_index(0); ++u) { unsigned int index = MAP_IDX(cost_map.info.width, u, v); int checksum_grid_map = 0; cost_map.data[index] = UNKNOWN_CELL; // grid map if (map_level & GRID_MAP) { switch (grid_map_.data[index]) { case OCCUPIED_CELL: if (map_level & ELEVATION_MAP && allow_elevation_map_to_clear_occupied_cells) { checksum_grid_map += grid_map_.at<int8_t>(v-1, u ); checksum_grid_map += grid_map_.at<int8_t>(v+1, u ); checksum_grid_map += grid_map_.at<int8_t>(v, u-1); checksum_grid_map += grid_map_.at<int8_t>(v, u+1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1); checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1); } cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } // point cloud if (map_level & CLOUD_MAP) { if (cost_map.data[index] != OCCUPIED_CELL) { switch (cloud_cost_map.data[index]) { case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } } // elevation map if (map_level & ELEVATION_MAP) { if (cost_map.data[index] != OCCUPIED_CELL || (allow_elevation_map_to_clear_occupied_cells && checksum_grid_map <= max_clear_size*OCCUPIED_CELL)) { switch (elevation_cost_map.data[index]) { case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break; case FREE_CELL: cost_map.data[index] = FREE_CELL; break; } } } } } ROS_DEBUG("HectorCM: computed a new costmap"); return true; }
void TeleopTurtle::keyLoop() { char c; bool dirty=false; // get the console in raw mode tcgetattr(kfd, &cooked); memcpy(&raw, &cooked, sizeof(struct termios)); raw.c_lflag &=~ (ICANON | ECHO); // Setting a new line, then end of file raw.c_cc[VEOL] = 1; raw.c_cc[VEOF] = 2; tcsetattr(kfd, TCSANOW, &raw); puts("Reading from keyboard"); puts("---------------------------"); puts("Up/Down : change speed by +/- 0.5 cm/s"); puts("Left/Right : change steer by +/- 10 deg"); linear_=angular_=0; for(;;) { // get the next event from the keyboard if(read(kfd, &c, 1) < 0) { perror("read():"); exit(-1); } ROS_DEBUG("value: 0x%02X\n", c); switch(c) { case KEYCODE_L: ROS_DEBUG("LEFT"); angular_ -= 10.0; dirty = true; break; case KEYCODE_R: ROS_DEBUG("RIGHT"); angular_ += 10.0; dirty = true; break; case KEYCODE_U: ROS_DEBUG("UP"); linear_ += 0.5; dirty = true; break; case KEYCODE_D: ROS_DEBUG("DOWN"); linear_ -= 0.5; dirty = true; break; case 's': case 'S': linear_ = angular_ = 0; dirty = true; break; } bb_msgs::DriveCmd vel; vel.steerAngle = a_scale_*angular_ * 3.141592654 / 180; vel.velocity = l_scale_*linear_; if(dirty ==true) { printf("velocity: %.1f cm/s\tsteer: %.1f deg\n", linear_, angular_); vel_pub_.publish(vel); dirty=false; } } return; }
Timer::Impl::~Impl() { ROS_DEBUG("Timer deregistering callbacks."); stop(); }
void ScitosCharger::reconfigure_callback( scitos_mira::ChargerParametersConfig& config, uint32_t level) { ROS_DEBUG("Reconfigure request on ScitosCharger module."); //Set the MIRA parameters to what was selected... // TODO: Decide if this is a good idea: all parameters relate to contact control, which might be dangerous and not needed. }
void CameraDisplay::updateCamera() { sensor_msgs::CameraInfo::ConstPtr info; sensor_msgs::Image::ConstPtr image; { boost::mutex::scoped_lock lock( caminfo_mutex_ ); info = current_caminfo_; image = texture_.getImage(); } if( !info || !image ) { return; } if( !validateFloats( *info )) { setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" ); return; } Ogre::Vector3 position; Ogre::Quaternion orientation; context_->getFrameManager()->getTransform( image->header.frame_id, ros::Time(0), position, orientation ); //printf( "CameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z ); // convert vision (Z-forward) frame to ogre frame (Z-out) orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X ); float img_width = info->width; float img_height = info->height; // If the image width is 0 due to a malformed caminfo, try to grab the width from the image. if( img_width == 0 ) { ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() )); img_width = texture_.getWidth(); } if (img_height == 0) { ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() )); img_height = texture_.getHeight(); } if( img_height == 0.0 || img_width == 0.0 ) { setStatus( StatusProperty::Error, "Camera Info", "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" ); return; } double fx = info->P[0]; double fy = info->P[5]; float win_width = render_panel_->width(); float win_height = render_panel_->height(); float zoom_x = zoom_property_->getFloat(); float zoom_y = zoom_x; // Preserve aspect ratio if( win_width != 0 && win_height != 0 ) { float img_aspect = (img_width/fx) / (img_height/fy); float win_aspect = win_width / win_height; if ( img_aspect > win_aspect ) { zoom_y = zoom_y / img_aspect * win_aspect; } else { zoom_x = zoom_x / win_aspect * img_aspect; } } // Add the camera's translation relative to the left camera (from P[3]); double tx = -1 * (info->P[3] / fx); Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X; position = position + (right * tx); double ty = -1 * (info->P[7] / fy); Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y; position = position + (down * ty); if( !validateFloats( position )) { setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" ); return; } render_panel_->getCamera()->setPosition( position ); render_panel_->getCamera()->setOrientation( orientation ); // calculate the projection matrix double cx = info->P[2]; double cy = info->P[6]; double far_plane = 100; double near_plane = 0.01; Ogre::Matrix4 proj_matrix; proj_matrix = Ogre::Matrix4::ZERO; proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x; proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y; proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x; proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y; proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane); proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane); proj_matrix[3][2]= -1; render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix ); setStatus( StatusProperty::Ok, "Camera Info", "OK" ); #if 0 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01); debug_axes->setPosition(position); debug_axes->setOrientation(orientation); #endif //adjust the image rectangles to fit the zoom & aspect ratio bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y ); fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y ); Ogre::AxisAlignedBox aabInf; aabInf.setInfinite(); bg_screen_rect_->setBoundingBox( aabInf ); fg_screen_rect_->setBoundingBox( aabInf ); }