bool checkNextStep() {
printField();
struct position nextPosition;
nextPosition.i = currentPosition.i + shiftList[currentDirection][0];
nextPosition.j = currentPosition.j + shiftList[currentDirection][1];
enum state_type next;
if (nextPosition.i < 0 || nextPosition.j < 0 || nextPosition.i >= field.cols ||
nextPosition.j >= field.rows) {
next = WALL;
} else {
next = field.m[currentPosition.i + shiftList[currentDirection][0]]
[currentPosition.j + shiftList[currentDirection][1]];
}
if (next != NONE) {
char res[25];
currentSensor = next;
if (currentState == GO) {
currentState = STOP;
sprintf(res, "Stopped: %s", sensor_type_str[currentSensor]);
} else if (currentState == STOP) {
sprintf(res, "The %s was detected", sensor_type_str[currentSensor]);
}
publishRobotState();
publishMessage(res);
return false;
}
currentSensor = next;
return true;
}
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses,
const moveit::core::JointModelGroup* group)
{
std::vector<std::string> tips;
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
tips.push_back(arm_datas_[i].ee_link_->getName());
// ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName());
const double timeout = 1.0 / 30.0; // 30 fps
// Optionally collision check
moveit::core::GroupStateValidityCallbackFn constraint_fn;
if (true)
{
bool collision_checking_verbose_ = false;
bool only_check_self_collision_ = false;
// TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults
// TODO(davetcoleman): change to std shared_ptr
boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_));
constraint_fn = boost::bind(&isIKStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(),
collision_checking_verbose_, only_check_self_collision_, visual_tools_, _1, _2, _3);
}
// Solve
std::size_t outer_attempts = 20;
for (std::size_t i = 0; i < outer_attempts; ++i)
{
if (!imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn))
{
ROS_DEBUG_STREAM_NAMED(name_, "Failed to find dual arm pose, trying again");
// Re-seed
imarker_state_->setToRandomPositions(group);
}
else
{
ROS_DEBUG_STREAM_NAMED(name_, "Found IK solution");
// Visualize robot
publishRobotState();
// Update the imarkers
for (IMarkerEndEffectorPtr ee : end_effectors_)
ee->setPoseFromRobotState();
return true;
}
}
ROS_ERROR_STREAM_NAMED(name_, "Failed to find dual arm pose");
return false;
}
IMarkerRobotState::IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm,
const std::string& imarker_name, std::vector<ArmData> arm_datas,
rviz_visual_tools::colors color, const std::string& package_path)
: name_(imarker_name), nh_("~"), psm_(psm), arm_datas_(arm_datas), color_(color), package_path_(package_path)
{
// Load Visual tools
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
psm_->getRobotModel()->getModelFrame(), nh_.getNamespace() + "/" + imarker_name, psm_);
// visual_tools_->setPlanningSceneMonitor(psm_);
visual_tools_->loadRobotStatePub(nh_.getNamespace() + "/imarker_" + imarker_name + "_state");
// Load robot state
imarker_state_ = std::make_shared<moveit::core::RobotState>(psm_->getRobotModel());
imarker_state_->setToDefaultValues();
// Create Marker Server
const std::string imarker_topic = nh_.getNamespace() + "/" + imarker_name + "_imarker";
imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(imarker_topic, "", false);
// Get file name
if (!getFilePath(file_path_, "imarker_" + name_ + ".csv", "config/imarkers"))
exit(-1);
// Load previous pose from file
if (!loadFromFile(file_path_))
ROS_INFO_STREAM_NAMED(name_, "Unable to find state from file, setting to default");
// Show initial robot state loaded from file
publishRobotState();
// Create each end effector
end_effectors_.resize(arm_datas_.size());
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
{
std::string eef_name;
if (i == 0)
eef_name = imarker_name + "_right";
else
eef_name = imarker_name + "_left";
end_effectors_[i] = std::make_shared<IMarkerEndEffector>(this, eef_name, arm_datas_[i], color);
// Create map from eef name to object
name_to_eef_[eef_name] = end_effectors_[i];
}
// After both end effectors have been added, apply on server
imarker_server_->applyChanges();
ROS_DEBUG_STREAM_NAMED(name_, "IMarkerRobotState '" << name_ << "' Ready.");
}
void handleCommand() {
IOT_INFO("current state %d %s", currentState, state_type_str[currentState]);
if (currentState == GO) { // GO
if (currentCommand != COMMAND_STOP) {
publishMessage("Prohibited command. Only STOP available");
} else {
currentState = STOP;
IOT_INFO("stopped");
currentSensor = NONE;
publishRobotState();
publishMessage("Stopped");
}
}
else if (currentState == STOP) { // STOP
IOT_INFO("current command %d %s", currentCommand, protocol_str[currentCommand]);
if (currentCommand == COMMAND_LEFT || currentCommand == COMMAND_RIGHT) {
turn(currentCommand);
publishRobotState();
checkNextStep();
} else if (currentCommand == COMMAND_GO) {
if (checkNextStep()) {
// makeStep();
currentState = GO;
// currentSensor = NONE;
// publishRobotState();
}
} else if (currentCommand == COMMAND_STOP) {
publishMessage("I am not moving");
}
}
if (currentState == STOP) {
publishMessage(getFieldAsString());
}
}
void makeStep() {
if (currentState != STOP) {
currentPosition.i += shiftList[currentDirection][0];
currentPosition.j += shiftList[currentDirection][1];
publishRobotState();
char* fieldStr = getFieldAsString();
char* res = (char*) malloc(MAX_LENGTH_OF_UPDATE_JSON_BUFFER*sizeof(char));
strcpy(res, "");
strcat(res, "I am going\n");
strcat(res, fieldStr);
publishMessage(res);
free(res);
}
}
void iot_subscribe_callback_handler(AWS_IoT_Client *pClient, char *topicName, uint16_t topicNameLen,
IoT_Publish_Message_Params *params, void *pData) {
IOT_UNUSED(pData);
IOT_UNUSED(pClient);
IOT_INFO("Message: %.*s\t%.*s", topicNameLen, topicName, (int) params->payloadLen, (char*)params->payload);
char* msg = (char*)(malloc((int) params->payloadLen) + 1);
strncpy(msg, (char*) params->payload, (int) params->payloadLen);
msg[(int) params->payloadLen] = '\0';
if (findCommand(msg)) {
handleCommand();
} else {
publishRobotState();
publishMessage("Undefined command");
}
}
int main(int argc, char **argv) {
IoT_Error_t rc = FAILURE;
parseInputArgsForConnectParams(argc, argv);
if (!readDataFromFile()) {
printf("Config file '%s' not found. Robot cannot work without configuration.\n", filename);
return -1;
}
else {
if (connectToThingAndSubscribeToTopic(argc, argv)) {
publishMessage(getFieldAsString());
publishRobotState();
while (true) {
//Max time the yield function will wait for read messages
rc = aws_iot_mqtt_yield(&mqttClient, 1000);
if (NETWORK_ATTEMPTING_RECONNECT == rc) { // || NETWORK_ATTEMPTING_RECONNECT == rc1) {
// If the client is attempting to reconnect we will skip the rest of the loop.
continue;
}
sleep(3);
if (currentState == GO) {
if (checkNextStep()) {
currentState = GO;
currentSensor = NONE;
makeStep();
}
}
}
}
return 0;
}
}