MStatus sgBDataCmd_key::readData( MString nameFilePath )
{
MStatus status;
std::ifstream inFile( nameFilePath.asChar(), ios::binary );
MString nameTarget;
readString( nameTarget, inFile );
MSelectionList selList;
nameTarget.substitute( ":", "_" );
MGlobal::getSelectionListByName( nameTarget, selList );
MObject oTarget;
selList.getDependNode( 0, oTarget );
MFnDagNode fnNode( oTarget );
m_objectKeyDataImport.oTargetNode = oTarget;
readUnsignedInt( m_objectKeyDataImport.unit, inFile );
readStringArray( m_objectKeyDataImport.namesAttribute, inFile );
readDoubleArray( m_objectKeyDataImport.dArrTime, inFile );
unsigned int lengthValues = m_objectKeyDataImport.namesAttribute.length() * m_objectKeyDataImport.dArrTime.length();
m_objectKeyDataImport.dArrValuesArray.setLength( lengthValues );
for( unsigned int j=0; j<lengthValues; j++ )
inFile.read( ( char* )&m_objectKeyDataImport.dArrValuesArray[j], sizeof( double ) );
inFile.close();
return MS::kSuccess;
}
void TestCollisionWorld::createObjectList(XmlRpc::XmlRpcValue list, int type)
{
for (int32_t i = 0; i < list.size(); ++i)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("Parameter needs to be an array of structs");
return;
}
std::string name = list[i]["name"];
std::string frame = list[i]["frame"];
std::vector<double> position, orientation, dimensions;
readDoubleArray(list[i]["position"], position);
readDoubleArray(list[i]["orientation"], orientation);
readDoubleArray(list[i]["dimensions"], dimensions);
createObject(type, name, frame, position, orientation, dimensions);
}
}
void TestCollisionWorld::createObjectsFromParamServer(ros::NodeHandle& node_handle)
{
XmlRpc::XmlRpcValue list;
if (node_handle.getParam("meshes", list))
{
for (int32_t i = 0; i < list.size(); ++i)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("Parameter %s/meshes needs to be an array of structs", node_handle.getNamespace().c_str());
return;
}
std::string name = list[i]["name"];
std::string url = list[i]["url"];
std::string frame = list[i]["frame"];
std::vector<double> position, orientation, scale;
readDoubleArray(list[i]["position"], position);
readDoubleArray(list[i]["orientation"], orientation);
readDoubleArray(list[i]["scale"], scale);
createMeshObject(name, url, frame, position, orientation, scale);
}
}
if (node_handle.getParam("cylinders", list))
{
createObjectList(list, geometric_shapes_msgs::Shape::CYLINDER);
}
if (node_handle.getParam("boxes", list))
{
createObjectList(list, geometric_shapes_msgs::Shape::BOX);
}
if (node_handle.getParam("spheres", list))
{
createObjectList(list, geometric_shapes_msgs::Shape::SPHERE);
}
// HACK
if (node_handle.hasParam("attached_object"))
{
//add a cylinder into the collision space attached to the r_gripper_r_finger_tip_link
mapping_msgs::AttachedCollisionObject att_object;
att_object.link_name = "r_gripper_palm_link";
//att_object.touch_links.push_back("r_gripper_palm_link");
att_object.touch_links.push_back("r_gripper_r_finger_link");
att_object.touch_links.push_back("r_gripper_l_finger_link");
att_object.touch_links.push_back("r_gripper_l_finger_tip_link");
att_object.touch_links.push_back("r_gripper_r_finger_tip_link");
att_object.object.id = "attached_can";
att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
att_object.object.header.frame_id = "r_gripper_tool_frame";
att_object.object.header.stamp = ros::Time::now();
geometric_shapes_msgs::Shape object;
object.type = geometric_shapes_msgs::Shape::CYLINDER;
object.dimensions.resize(2);
object.dimensions[0] = .03;
object.dimensions[1] = 0.15;
geometry_msgs::Pose pose;
pose.position.x = 0.0;
pose.position.y = 0.0;
pose.position.z = 0.07;
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
pose.orientation.w = 1;
att_object.object.shapes.push_back(object);
att_object.object.poses.push_back(pose);
attached_object_pub_.publish(att_object);
}
}
