std::pair<
typename viennagrid::result_of::container_of<container_collection_type, value_type>::type::handle_type,
bool
>
physical_insert( value_type element, inserter_type & inserter )
{
typedef typename viennagrid::result_of::container_of<container_collection_type, value_type>::type container_type;
typedef typename viennagrid::result_of::container_of<container_collection_type, value_type>::type::handle_type handle_type;
container_type & container = viennagrid::get< value_type >( *collection );
if ( generate_id && !container.is_present( element ) )
viennagrid::detail::set_id(element, id_generator( viennagrid::detail::tag<value_type>() ) );
if (!generate_id)
id_generator.set_max_id( element.id() );
std::pair<handle_type, bool> ret = container.insert( element );
if (change_counter) ++(*change_counter);
if (call_callback)
viennagrid::detail::insert_callback(
container.dereference_handle(ret.first),
ret.second,
inserter);
inserter.handle_insert( ret.first, viennagrid::detail::tag<value_type>() );
return ret;
}
/**
* create a polygon marker based on line marker
* @param poly polygon from geometry library
* @param scale thickness of the line
* @param name polygon's name
* @return marker line marker representating input polygon
*/
visualization_msgs::Marker definePolygon(geometry_msgs::Polygon poly, double scale, std::string name) {
//declarration
visualization_msgs::Marker marker;
//frame id
marker.header.frame_id = "map";
//namespace
std::ostringstream nameSpace;
nameSpace << name;
marker.ns = nameSpace.str();
marker.id = id_generator(name); //creation of an unique id based on marker's name
//action
marker.action = visualization_msgs::Marker::ADD;
//orientation
marker.pose.orientation.w = 1.0;
//color
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
//points
geometry_msgs::Point p;
for (int i = 0; i < poly.points.size(); i++) {
geometry_msgs::Point p;
p.x = poly.points[i].x;
p.y = poly.points[i].y;
p.z = poly.points[i].z;
marker.points.push_back(p);
}
p.x = poly.points[0].x;
p.y = poly.points[0].y;
p.z = poly.points[0].z;
marker.points.push_back(p);
//scale
marker.scale.x = scale;
//type
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.lifetime = ros::Duration();
return marker;
}
/**
* create a robot marker
* @param x coordinates of robot's base in the x direction
* @param y coordinates of robot's base in the y direction
* @param z coordinates of robot's base in the z direction
* @param scale dimension of the marker
* @param name marker's name
* @return marker mesh marker of robot
*/
visualization_msgs::Marker defineRobot(double x, double y, double z, double roll, double pitch, double yaw, double scale, std::string name) {
//declarration
visualization_msgs::Marker marker;
//frame id
marker.header.frame_id = "map";
//namespace
std::ostringstream nameSpace;
nameSpace << name;
marker.ns = nameSpace.str();
marker.id = id_generator(name); //creation of an unique id based on marker's name
//action
marker.action = visualization_msgs::Marker::ADD;
//position
marker.pose.position.x = x;
marker.pose.position.y = y;
marker.pose.position.z = z + 0.1;
//orientation
marker.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
//color
marker.color.r = 0.0f;
marker.color.g = 0.0f;
marker.color.b = 0.0f;
marker.color.a = 0.0;
//scale
marker.scale.x = 1 * scale;
marker.scale.y = 1 * scale;
marker.scale.z = 1 * scale;
//type de marker
marker.type = visualization_msgs::Marker::MESH_RESOURCE;
marker.mesh_resource = "package://toaster_visualizer/mesh/toaster_robots/pr2.dae"; //using 3d robot model
marker.mesh_use_embedded_materials = true;
marker.lifetime = ros::Duration();
return marker;
}
/**
* create a circle marker
* @param p point from geometry library locating the center of the circle
* @param rayon radius of the circle
* @param name marker's name
* @return marker circle marker with input property
*/
visualization_msgs::Marker defineCircle(geometry_msgs::Point p, double rayon, std::string name) {
//declarration
visualization_msgs::Marker marker;
//frame id
marker.header.frame_id = "map";
//namespace
std::ostringstream nameSpace;
nameSpace << name;
marker.ns = nameSpace.str();
marker.id = id_generator(name); //creation of an unique id based on marker's name
//action
marker.action = visualization_msgs::Marker::ADD;
//position
marker.pose.position.x = p.x;
marker.pose.position.y = p.y;
marker.pose.position.z = p.z;
//orientation
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
//color
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
//dimemsion
marker.scale.x = rayon;
marker.scale.y = rayon;
marker.scale.z = 0.1;
//type
marker.type = 2;
marker.lifetime = ros::Duration();
return marker;
}
/**
* CallBack creating markers based on received toaster_msgs and adding then to human_list
* Humans can be representated by a single unarticulated mesh or by mutiple meshs for an articulated model
* @param msg reference to receive toaster_msgs::HumanList
* @return void
*/
void chatterCallbackHumanList(const toaster_msgs::HumanList::ConstPtr& msg) {
human_list.markers.clear();
for (int i = 0; i < msg->humanList.size(); i++) {
//non articulated human
visualization_msgs::Marker m = defineHuman(msg->humanList[i].meAgent.meEntity.positionX, msg->humanList[i].meAgent.meEntity.positionY, msg->humanList[i].meAgent.meEntity.positionZ, msg->humanList[i].meAgent.meEntity.orientationRoll, msg->humanList[i].meAgent.meEntity.orientationPitch, msg->humanList[i].meAgent.meEntity.orientationYaw,
0.3, msg->humanList[i].meAgent.meEntity.name);
visualization_msgs::Marker mn = defineName(m);
mn = setPosition(mn, mn.pose.position.x, mn.pose.position.y, 3);
mn = setSize(mn, 0, 0, 0.5);
mn = setColor(mn, 1.0, 1.0, 1.0);
human_list.markers.push_back(mn);
if (msg->humanList[i].meAgent.skeletonJoint.size() == 0) {
human_list.markers.push_back(m);
ROS_DEBUG("human %d", m.id);
} else //articulated human
{
std::vector<toaster_msgs::Joint> joints = msg->humanList[i].meAgent.skeletonJoint;
visualization_msgs::Marker markerTempo;
int scale = 1;
for (int y = 0; y < joints.size(); y++) {
ROS_DEBUG("joint");
std::string name = msg->humanList[i].meAgent.skeletonJoint[y].meEntity.name;
//frame id
markerTempo.header.frame_id = "map";
//namespace
markerTempo.ns = name;
markerTempo.id = id_generator(joints[y].meEntity.name); //creation of an unique id based on marker's name
//action
markerTempo.action = visualization_msgs::Marker::ADD;
//position
markerTempo = setPosition(markerTempo, joints[y].meEntity.positionX, joints[y].meEntity.positionY, joints[y].meEntity.positionZ);
//orientation
markerTempo = setOrientation(markerTempo, joints[y].meEntity.orientationRoll, joints[y].meEntity.orientationPitch, joints[y].meEntity.orientationYaw);
//color
markerTempo.color.r = 1.0f;
markerTempo.color.g = 1.0f;
markerTempo.color.b = 1.0f;
markerTempo.color.a = 1.0;
//scale
markerTempo.scale.x = 0.1 * scale;
markerTempo.scale.y = 0.1 * scale;
markerTempo.scale.z = 0.1 * scale;
//type de marker
markerTempo.type = 3;
TiXmlHandle hdl(&listMemb);
TiXmlElement *elem = hdl.FirstChildElement().FirstChildElement().Element();
std::string name_obj;
std::string mesh_r;
while (elem) //for each element of the xml file
{
name_obj = elem->Attribute("name");
mesh_r = elem->Attribute("mesh_ressource");
elem = elem->NextSiblingElement();
if (name_obj.compare(name) == 0) //if there is a 3d model related to this object
{
markerTempo.scale.x = 1 * scale;
markerTempo.scale.y = 1 * scale;
markerTempo.scale.z = 1 * scale;
markerTempo.color.r = 0.0f;
markerTempo.color.g = 0.0f;
markerTempo.color.b = 0.0f;
markerTempo.color.a = 0.0;
markerTempo.type = visualization_msgs::Marker::MESH_RESOURCE; //use it as mesh
markerTempo.mesh_resource = mesh_r;
markerTempo.mesh_use_embedded_materials = true;
elem = NULL;
}
}
markerTempo.lifetime = ros::Duration();
human_list.markers.push_back(markerTempo);
}
}
}
}
/**
* create an object marker
* @param x coordinates of object's base in thx x direction
* @param y coordinates of object's base in thx y direction
* @param z coordinates of object's base in thx z direction
* @param scale dimension of the marker
* @param name marker's name
* @return marker object marker or mesh marker if the object is in the mesh database
*/
visualization_msgs::Marker defineObj(double x, double y, double z, double scale, std::string name) {
//declarration
visualization_msgs::Marker marker;
//frame id
marker.header.frame_id = "map";
//namespace
std::ostringstream nameSpace;
nameSpace << name;
marker.ns = nameSpace.str();
marker.id = id_generator(name); //creation of an unique id based on marker's name
//action
marker.action = visualization_msgs::Marker::ADD;
//position
marker.pose.position.x = x;
marker.pose.position.y = y;
marker.pose.position.z = z;
//orientation
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
//color
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 1.0f;
marker.color.a = 1.0;
//scale
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
//type
marker.type = visualization_msgs::Marker::CUBE; //marker by defaut
//std::stringstream s;
//s << ros::package::getPath("toaster_visualizer") << "/src/list_obj.xml";
//TiXmlDocument list(s.str()); //load xml file
//TiXmlDocument list("src/toaster/toaster_visualizer/src/list_obj.xml"); //load xml file
TiXmlHandle hdl(&listObj);
TiXmlElement *elem = hdl.FirstChildElement().FirstChildElement().Element();
std::string name_obj;
std::string mesh_r;
while (elem) //for each element of the xml file
{
name_obj = elem->Attribute("name");
mesh_r = elem->Attribute("mesh_ressource");
elem = elem->NextSiblingElement();
if (name_obj.compare(name) == 0) //if there is a 3d model relativ to this object
{
marker.scale.x = 1 * scale;
marker.scale.y = 1 * scale;
marker.scale.z = 1 * scale;
marker.color.r = 0.0f;
marker.color.g = 0.0f;
marker.color.b = 0.0f;
marker.color.a = 0.0;
marker.type = visualization_msgs::Marker::MESH_RESOURCE; //use it as mesh
marker.mesh_resource = mesh_r;
marker.mesh_use_embedded_materials = true;
elem = NULL;
}
}
marker.lifetime = ros::Duration();
return marker;
}
