void
ArxDbgUiTdcObjReactorsBase::displayObjList()
{
m_objsAttached.setLogicalLength(0);
m_lbObjList.ResetContent();
getAttachedObjects(m_objsAttached);
CString str;
Acad::ErrorStatus es;
AcDbObject* obj;
int len = m_objsAttached.length();
for (int i=0; i<len; i++) {
es = acdbOpenAcDbObject(obj, m_objsAttached[i], AcDb::kForRead, true);
if (es == Acad::eOk) {
ArxDbgUtils::objToClassAndHandleStr(obj, str);
if (obj->isErased())
str += _T(" (erased)");
m_lbObjList.AddString(str);
obj->close();
}
}
setButtonModes();
}
bp::dict getAttachedObjectsPython(const bp::list& object_ids)
{
std::map<std::string, moveit_msgs::AttachedCollisionObject> aobjs =
getAttachedObjects(py_bindings_tools::stringFromList(object_ids));
std::map<std::string, std::string> ser_aobjs;
for (std::map<std::string, moveit_msgs::AttachedCollisionObject>::const_iterator it = aobjs.begin();
it != aobjs.end(); ++it)
ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second);
return py_bindings_tools::dictFromType(ser_aobjs);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "publish_objects_tf_frames");
ros::NodeHandle n;
tf::TransformBroadcaster br;
std::vector<moveit_msgs::CollisionObject> cos;
std::vector<moveit_msgs::AttachedCollisionObject> acos;
// boost::this_thread::sleep(boost::posix_time::seconds(3));
// geometry_msgs::PoseStamped cam_pose = getCamPose(n);
geometry_msgs::PoseStamped temp_pose;
ROS_INFO_STREAM("tf publisher started..........");
ros::service::waitForService(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
ros::ServiceClient ps_service_client = n.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
ros::WallDuration(1.0).sleep();
suturo_manipulation::NodeStatus node_status(n);
node_status.nodeStarted(suturo_manipulation_msgs::ManipulationNodeStatus::NODE_PUBLISH_OBJECT_FRAMES);
while (getObjects(ps_service_client, cos) && getAttachedObjects(ps_service_client, acos))
{
//publish tf frame in every collisionobject
// if ()
// {
// ROS_INFO_STREAM(cos.size());
for (std::vector<moveit_msgs::CollisionObject>::iterator co = cos.begin(); co != cos.end(); ++co)
{
if (co->primitive_poses.size() > 0)
{
temp_pose.pose = co->primitive_poses[0];
temp_pose.header = co->header;
publishTfFrame(co->id, temp_pose, br);
}
}
// }
// if (getAttachedObjects(ps_service_client, acos))
// {
for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator co = acos.begin(); co != acos.end(); ++co)
{
if (co->object.primitive_poses.size() > 0)
{
temp_pose.pose = co->object.primitive_poses[0];
temp_pose.header = co->object.header;
publishTfFrame(co->object.id, temp_pose, br);
}
}
// }
//publish tf frame in every attachedobject
// for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator aco = acos.begin(); aco != acos.end(); ++aco)
// {
// if (aco->object.primitive_poses.size() >= 1)
// {
// ROS_DEBUG_STREAM(*aco);
// temp_pose.pose = aco->object.primitive_poses[0];
// temp_pose.header = aco->object.header;
// publishTfFrame(aco->object.id, temp_pose, transform, br);
// }
// else
// {
// ROS_DEBUG_STREAM(*aco);
// temp_pose.pose = aco->object.mesh_poses[0];
// temp_pose.header = aco->object.header;
// publishTfFrame(aco->object.id, temp_pose, transform, br);
// }
// }
//publish cam_frame
// publishTfFrame("webcam", cam_pose, transform, br);
ros::WallDuration(0.1).sleep();
}
return 0;
}