void userCallback(InteractiveRobot& robot)
{
// move the world geometry in the collision world
Eigen::Affine3d world_cube_pose;
double world_cube_size;
robot.getWorldGeometry(world_cube_pose, world_cube_size);
g_planning_scene->getWorldNonConst()->moveShapeInObject("world_cube", g_world_cube_shape, world_cube_pose);
// prepare to check collisions
collision_detection::CollisionRequest c_req;
collision_detection::CollisionResult c_res;
c_req.group_name = "right_gripper";
//c_req.group_name = "right_arm";
c_req.contacts = true;
c_req.max_contacts = 100;
c_req.max_contacts_per_pair = 5;
c_req.verbose = true;
// check for collisions between robot and itself or the world
g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());
// display results of the collision check
if (c_res.collision)
{
ROS_INFO("COLLIDING contact_point_count=%d",(int)c_res.contact_count);
// get the contact points and display them as markers
if (c_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
color.r = 1.0;
color.g = 0.0;
color.b = 1.0;
color.a = 0.5;
visualization_msgs::MarkerArray markers;
collision_detection::getCollisionMarkersFromContacts(markers,
"base_footprint",
c_res.contacts,
color,
ros::Duration(), // remain until deleted
0.01); // radius
publishMarkers(markers);
}
}
else
{
ROS_INFO("Not colliding");
// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
}
}
bool ObjectMatching::matchCallback(tangible_msgs::GetMatchingObjects::Request& req,
tangible_msgs::GetMatchingObjects::Response& res)
{
ROS_INFO("Got service call!");
if (req.target.type == tangible_msgs::Target::POINT_LOCATION){
ROS_INFO("Matching object for target type POINT_LOCATION");
for (int i=0; i<req.objects.size(); i++){
if (matchesPointLocation(req.objects[i], req.target)){
res.objects.push_back(req.objects[i]);
}
}
}
else if (req.target.type == tangible_msgs::Target::REGION) {
ROS_INFO("Matching object for target type REGION");
for (int i=0; i<req.objects.size(); i++){
if (matchesRegion(req.objects[i], req.target)){
res.objects.push_back(req.objects[i]);
}
}
}
else if (req.target.type == tangible_msgs::Target::OBJECT_SELECTOR) {
ROS_INFO("Matching object for target type OBJECT_SELECTOR");
for (int i=0; i<req.objects.size(); i++){
if (matchesObjSelector(req.objects[i], req.target)){
res.objects.push_back(req.objects[i]);
}
}
}
publishMarkers();
return true;
}
bool HapticsPSM::check_collison(){
coll_res.clear();
psm_planning_scene->checkCollisionUnpadded(coll_req,coll_res,*group->getCurrentState(),
psm_planning_scene->getAllowedCollisionMatrix());
if (coll_res.collision)
{
//ROS_INFO("COLLIDING contact_point_count=%d",(int)coll_res.contact_count);
if (coll_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
color.r = 1.0;
color.g = 0.0;
color.b = 0.2;
color.a = 0.8;
visualization_msgs::MarkerArray markers;
collision_detection::getCollisionMarkersFromContacts(markers,
group->getPlanningFrame().c_str(),
coll_res.contacts,
color,
ros::Duration(), // remain until deleted
0.002);
publishMarkers(markers);
}
return true;
}
else
{
//ROS_INFO("Not colliding");
// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
return false;
}
}
void WolfNode::solve()
{
// Solving ---------------------------------------------------------------------------
//ROS_INFO("================ SOLVING ==================");
ceres::Solver::Summary summary = ceres_manager_.solve();
//std::cout << "------------------------- SOLVED -------------------------" << std::endl;
//std::cout << summary.BriefReport() << std::endl; //summary.FullReport()
// Broadcast transforms ---------------------------------------------------------------------------
broadcastTf();
// MARKERS VEHICLE & CONSTRAINTS ---------------------------------------------------------------------------
publishMarkers();
}
bool AudioProcessor::processFrame()
{
// TODO: check whether this reallocates memory
frequency_histogram_.setZero(static_cast<Eigen::DenseIndex>(number_of_frequency_bins_));
for (int i = 0; i < num_frequencies_bins_; i++)
{
for (int j = 0; j < num_frequencies_per_bin_; ++j)
{
frequency_histogram_(i) = frequency_histogram_(i) + spectrum_[i * num_frequencies_per_bin_ + j];
}
}
if(publish_visualization_markers_ && (frame_count_ % visualization_publishing_ratio_) == 0)
{
publishMarkers();
}
return true;
}
void LoadEstimator::publish_inertial_data()
{
while(going_)
{
mutex_thread_.lock();
double norm_of_P = P_.norm();
std::cout << "norm of P_ \t" << norm_of_P << std::endl << std::endl;
computeInertialParameters();
mutex_thread_.unlock();
//we don;t need the lock here
inertial_estimation_pub_.publish(inertial_parameters_msg_);
//Publish markers
publishMarkers();
ros::Duration(pub_time_).sleep();
}
}
