// Constructor
GraspGeneratorTest(int num_tests) :
nh_("~")
{
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, BASE_LINK));
rviz_tools_->setLifetime(40.0);
rviz_tools_->setMuted(false);
// ---------------------------------------------------------------------------------------------
// Load grasp generator
loadRobotGraspData(); // Load robot specific data
block_grasp_generator_.reset( new block_grasp_generator::BlockGraspGenerator(rviz_tools_) );
// ---------------------------------------------------------------------------------------------
// Load grasp filter
bool rviz_verbose = true;
grasp_filter_.reset(new block_grasp_generator::GraspFilter(BASE_LINK, rviz_verbose, rviz_tools_, PLANNING_GROUP_NAME) );
// ---------------------------------------------------------------------------------------------
// Generate grasps for a bunch of random blocks
geometry_msgs::Pose block_pose;
std::vector<manipulation_msgs::Grasp> possible_grasps;
// Loop
for (int i = 0; i < num_tests; ++i)
{
ROS_INFO_STREAM_NAMED("test","Adding random block " << i+1 << " of " << num_tests);
generateRandomBlock(block_pose);
//getTestBlock(block_pose);
rviz_tools_->publishBlock(block_pose, BLOCK_SIZE, false);
possible_grasps.clear();
// Generate set of grasps for one block
rviz_tools_->setMuted(true); // we don't want to see unfiltered grasps
block_grasp_generator_->generateGrasps( block_pose, grasp_data_, possible_grasps);
rviz_tools_->setMuted(false);
// Filter the grasp for only the ones that are reachable
grasp_filter_->filterGrasps(possible_grasps);
// Visualize them
block_grasp_generator_->visualizeGrasps(possible_grasps, block_pose);
// Make sure ros is still going
if(!ros::ok)
break;
}
}
void HandshakeResponder::responderRespondHello(){ //R:1 forge
QByteArray clearText;
clearText.append(SUPPORTED_PROTOCOL_VERSION);
QByteArray firstHalfSymKey = generateRandomBlock(32);
m_gcmKey.append(firstHalfSymKey.left(16)); //first half key
m_gcmBaseIV.append(firstHalfSymKey.right(16));//first half IV
clearText.append(firstHalfSymKey);
updateIntegrityHash(&m_responderIntegrityHash, clearText);
disconnect(m_socket, &AbstractLink::readyRead, this, 0);
connect(m_socket, &AbstractLink::readyRead,
this, &HandshakeResponder::responderParseHalfKeyAndResponderIntegrity);
m_socketStream << rsaEncrypt(clearText);
m_timeout.start();
}
