LRESULT CALLBACK
TimeBar::wnd_proc(HWND hWnd, UINT uMessage, WPARAM wParam, LPARAM lParam)
{
TimeBar *pThis = (TimeBar*)GetWindowLongPtr(hWnd, GWLP_USERDATA);
switch (uMessage)
{
case WM_NCCREATE:
{
LPCREATESTRUCT lpcs = (LPCREATESTRUCT)lParam;
pThis = (TimeBar *)( lpcs->lpCreateParams );
SetWindowLongPtr(hWnd, GWLP_USERDATA, (LONG_PTR)pThis);
SetWindowPos(hWnd, NULL, 0, 0, 0, 0,
SWP_NOMOVE | SWP_NOSIZE | SWP_NOZORDER | SWP_FRAMECHANGED );
}
break;
case WM_PAINT:
return pThis->on_paint();
case WM_LBUTTONUP:
SendMessage(pThis->deskband->get_command_window(), WM_USER + 1, 0, NULL);
break;
}
return DefWindowProc(hWnd, uMessage, wParam, lParam);
}
bool GRobotControllerItem::onPlaybackInitialized(double time)
{
if(controller->isPlayingMotion()){
controller->stopMotion();
}
playbackConnections.disconnect();
if(bodyItem){
BodyMotionItemPtr motionItem =
ItemTreeView::mainInstance()->selectedSubItem<BodyMotionItem>(bodyItem);
if(motionItem){
MultiValueSeqPtr qseq = motionItem->jointPosSeq();
if(qseq->numFrames() > 0 && qseq->numParts() == controller->numJoints()){
if(controller->setMotion(&qseq->at(0, 0), qseq->numFrames(), qseq->getTimeStep())){
TimeBar* timeBar = TimeBar::instance();
playbackConnections.add(
timeBar->sigPlaybackStarted().connect(
bind(&GRobotControllerItem::onPlaybackStarted, this, _1)));
playbackConnections.add(
timeBar->sigPlaybackStopped().connect(
bind(&GRobotControllerItem::onPlaybackStopped, this)));
}
}
}
}
return true;
}
void KinematicFaultCheckerImpl::apply()
{
bool processed = false;
ItemList<BodyMotionItem> items = ItemTreeView::mainInstance()->selectedItems<BodyMotionItem>();
if(items.empty()){
mes.notify(_("No BodyMotionItems are selected."));
} else {
for(size_t i=0; i < items.size(); ++i){
BodyMotionItem* motionItem = items.get(i);
BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>();
if(!bodyItem){
mes.notify(str(fmt(_("%1% is not owned by any BodyItem. Check skiped.")) % motionItem->name()));
} else {
mes.putln();
mes.notify(str(fmt(_("Applying the Kinematic Fault Checker to %1% ..."))
% motionItem->headItem()->name()));
dynamic_bitset<> linkSelection;
if(selectedJointsRadio.isChecked()){
linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem);
} else if(nonSelectedJointsRadio.isChecked()){
linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem);
linkSelection.flip();
} else {
linkSelection.resize(bodyItem->body()->numLinks(), true);
}
double beginningTime = 0.0;
double endingTime = motionItem->motion()->getTimeLength();
std::numeric_limits<double>::max();
if(onlyTimeBarRangeCheck.isChecked()){
TimeBar* timeBar = TimeBar::instance();
beginningTime = timeBar->minTime();
endingTime = timeBar->maxTime();
}
int n = checkFaults(bodyItem, motionItem, mes.cout(),
positionCheck.isChecked(),
velocityCheck.isChecked(),
collisionCheck.isChecked(),
linkSelection,
beginningTime, endingTime);
if(n > 0){
if(n == 1){
mes.notify(_("A fault has been detected."));
} else {
mes.notify(str(fmt(_("%1% faults have been detected.")) % n));
}
} else {
mes.notify(_("No faults have been detected."));
}
processed = true;
}
}
}
}
void SimulationBar::onStopSimulationClicked()
{
forEachSimulator(boost::bind(&SimulationBar::stopSimulation, this, _1));
TimeBar* timeBar = TimeBar::instance();
if(timeBar->isDoingPlayback()){
timeBar->stopPlayback();
}
}
void SimulationBar::onStopSimulationClicked()
{
forEachSimulator(std::bind(&SimulationBar::stopSimulation, this, _1));
TimeBar* timeBar = TimeBar::instance();
if(timeBar->isDoingPlayback()){
timeBar->stopPlayback();
}
pauseToggle->setChecked(false);
}
void SimulationBar::pauseSimulation(SimulatorItem* simulator)
{
if(pauseToggle->isChecked()){
if(simulator->isRunning())
simulator->pauseSimulation();
TimeBar* timeBar = TimeBar::instance();
if(timeBar->isDoingPlayback()){
timeBar->stopPlayback();
}
}else{
if(simulator->isRunning())
simulator->restartSimulation();
TimeBar::instance()->startPlaybackFromFillLevel();
}
}
TimeBar* TimeBar::spriteWithFile(const char *pszFileName)
{
TimeBar *pobSprite = new TimeBar();
//TimeBar *pobSprite = TimeBar::create();
if (pobSprite&& pobSprite->initWithFile(pszFileName))
{
//pobSprite->scheduleUpdate();
//pobSprite->setDistanceMoved(0);
//pobSprite->setPosition(ccp(640,650));
pobSprite->init();
pobSprite->autorelease();
return pobSprite;
}
CC_SAFE_DELETE(pobSprite);
return NULL;
}
LRESULT
CDeskBand::OnCopyData(PCOPYDATASTRUCT copy_data)
{
TRACE_ENTER("CDeskBand::OnCopyData");
m_LastCopyData = time(NULL);
if (copy_data->dwData == APPLET_MESSAGE_MENU
&& copy_data->cbData == sizeof(AppletMenuData))
{
m_AppletMenu = *((AppletMenuData *) copy_data->lpData);
m_HasAppletMenu = TRUE;
}
else if (m_TimerBox != NULL
&& copy_data->dwData == APPLET_MESSAGE_HEARTBEAT
&& copy_data->cbData == sizeof(AppletHeartbeatData))
{
AppletHeartbeatData *data = (AppletHeartbeatData *) copy_data->lpData;
m_TimerBox->set_enabled(data->enabled);
for (int s = 0; s < BREAK_ID_SIZEOF; s++)
{
m_TimerBox->set_slot(s, (BreakId) data->slots[s]);
}
for (int b = 0; b < BREAK_ID_SIZEOF; b++)
{
TimeBar *bar = m_TimerBox->get_time_bar(BreakId(b));
if (bar != NULL)
{
bar->set_text(data->bar_text[b]);
bar->set_bar_color((ITimeBar::ColorId) data->bar_primary_color[b]);
bar->set_secondary_bar_color((ITimeBar::ColorId) data->bar_secondary_color[b]);
bar->set_progress(data->bar_primary_val[b], data->bar_primary_max[b]);
bar->set_secondary_progress(data->bar_secondary_val[b], data->bar_secondary_max[b]);
}
}
m_TimerBox->update(false);
}
TRACE_EXIT();
return 0;
}
void WorldRosItem::start()
{
static bool initialized = false;
if (initialized) return;
world = this->findOwnerItem<WorldItem>();
if (world)
ROS_INFO("Found WorldItem: %s", world->name().c_str());
sim = SimulatorItem::findActiveSimulatorItemFor(this);
if (sim == 0) return;
std::string name = sim->name();
ROS_INFO("Found SimulatorItem: %s", name.c_str());
std::replace(name.begin(), name.end(), '-', '_');
rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(world->name()));
pub_clock_ = rosnode_->advertise<rosgraph_msgs::Clock>("/clock", 10);
pub_link_states_ = rosnode_->advertise<gazebo_msgs::LinkStates>("link_states", 10);
pub_model_states_ = rosnode_->advertise<gazebo_msgs::ModelStates>("model_states", 10);
TimeBar* timeBar = TimeBar::instance();
timeBar->sigTimeChanged().connect(boost::bind(&WorldRosItem::timeTick, this, _1));
std::string pause_physics_service_name("pause_physics");
ros::AdvertiseServiceOptions pause_physics_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
pause_physics_service_name,
boost::bind(&WorldRosItem::pausePhysics,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
pause_physics_service_ = rosnode_->advertiseService(pause_physics_aso);
std::string unpause_physics_service_name("unpause_physics");
ros::AdvertiseServiceOptions unpause_physics_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
unpause_physics_service_name,
boost::bind(&WorldRosItem::unpausePhysics,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
unpause_physics_service_ = rosnode_->advertiseService(unpause_physics_aso);
std::string reset_simulation_service_name("reset_simulation");
ros::AdvertiseServiceOptions reset_simulation_aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
reset_simulation_service_name,
boost::bind(&WorldRosItem::resetSimulation,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
reset_simulation_service_ = rosnode_->advertiseService(reset_simulation_aso);
std::string spawn_vrml_model_service_name("spawn_vrml_model");
ros::AdvertiseServiceOptions spawn_vrml_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_vrml_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_vrml_model_service_ = rosnode_->advertiseService(spawn_vrml_model_aso);
std::string spawn_urdf_model_service_name("spawn_urdf_model");
ros::AdvertiseServiceOptions spawn_urdf_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_urdf_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_urdf_model_service_ = rosnode_->advertiseService(spawn_urdf_model_aso);
std::string spawn_sdf_model_service_name("spawn_sdf_model");
ros::AdvertiseServiceOptions spawn_sdf_model_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::SpawnModel>(
spawn_sdf_model_service_name,
boost::bind(&WorldRosItem::spawnModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
spawn_sdf_model_service_ = rosnode_->advertiseService(spawn_sdf_model_aso);
std::string delete_model_service_name("delete_model");
ros::AdvertiseServiceOptions delete_aso =
ros::AdvertiseServiceOptions::create<gazebo_msgs::DeleteModel>(
delete_model_service_name,
boost::bind(&WorldRosItem::deleteModel,this,_1,_2),
ros::VoidPtr(), &rosqueue_);
delete_model_service_ = rosnode_->advertiseService(delete_aso);
async_ros_spin_.reset(new ros::AsyncSpinner(0));
async_ros_spin_->start();
rosqueue_thread_.reset(new boost::thread(&WorldRosItem::queueThread, this));
initialized = true;
}
void
TimerBox::update_time_bars(TransparentDamageControl &ctrl)
{
TRACE_ENTER("TimerBox::update_time_bars");
if (enabled)
{
int x = 0, y = 0;
int bar_w, bar_h;
int icon_width, icon_height;
TRACE_MSG("1");
slot_to_time_bar[0]->get_size(bar_w, bar_h);
break_to_icon[0]->get_size(icon_width, icon_height);
TRACE_MSG("2");
int icon_bar_w = icon_width + 2 * PADDING_X + bar_w;
int max_columns = __max(1, width / icon_bar_w);
int max_rows = __max(1, (height + PADDING_Y) / (__max(icon_height, bar_h) + PADDING_Y));
int rows = __max(1, __min(max_rows, (filled_slots + max_columns - 1) / max_columns));
int columns = (filled_slots + rows -1) / rows;
TRACE_MSG("3");
int box_h = rows * __max(icon_height, bar_h) + (rows - 1) * PADDING_Y;
y = __max(0, (height - box_h)/2);
int icon_dy = 0;
int bar_dy = 0;
TRACE_MSG("4");
if (bar_h > icon_height)
{
icon_dy = (bar_h - icon_height + 1) / 2;
}
else
{
bar_dy = (icon_height - bar_h + 1) / 2;
}
TRACE_MSG("5");
int current_column = 0;
for (int i = 0; i < BREAK_ID_SIZEOF; i++)
{
BreakId bid = slot_to_break[i];
TRACE_MSG("6");
if (bid != BREAK_ID_NONE)
{
TRACE_MSG("7");
TimeBar *bar = get_time_bar(bid);
ctrl.ShowWindow(break_to_icon[bid]->get_handle(), x, y + icon_dy);
ctrl.ShowWindow(bar->get_handle(), x+icon_width+PADDING_X, y + bar_dy);
bar->update();
x += icon_bar_w + 2 * PADDING_X;
current_column++;
if (current_column >= columns)
{
current_column = 0;
x = 0;
y += __max(icon_height, bar_h) + PADDING_Y;
}
}
}
}
TRACE_MSG("8");
for (int h = 0; h < BREAK_ID_SIZEOF; h++)
{
if ((! enabled) || (! break_visible[h]))
{
ctrl.HideWindow(break_to_icon[h]->get_handle());
}
}
TRACE_MSG("9");
for (int j = enabled ? filled_slots : 0; j < BREAK_ID_SIZEOF; j++)
{
ctrl.HideWindow(slot_to_time_bar[j]->get_handle());
}
TRACE_EXIT();
}
