void MapWidget::setViewOnRobot(std::string robot_id)
{
if (active_view_on_robot_ == robot_id)
{
return;
}
RobotGraphicsItem* robot = getRobot(robot_id);
QPointF position;
double heading;
getRobotPosition("/"+robot_id+"/base_link", position, heading);
graphicsView_->setTransform(QTransform());
graphicsView_->centerOn(position.x(),position.y());
// graphicsView_->scale(1, -1);
graphicsView_->scale(56, -56);
active_view_on_robot_ = robot_id;
}
Obstacle
SSLPathPlanner::getRobotAsObstacle (uint8_t team, uint8_t id, const geometry_msgs::Vector3& vel,
const geometry_msgs::Vector3& acc)
{
Obstacle obstacle = getRobotAsObstacle (team, id);
//assuming linear velocity in one control cycle which is about 20ms.
tf::Vector3 x_curr;
tf::vector3MsgToTF (getRobotPosition (team, id), x_curr);
tf::Vector3 v_curr;
tf::vector3MsgToTF (vel, v_curr);
tf::Vector3 acc_cmd;
tf::vector3MsgToTF (acc, acc_cmd);
tf::Vector3 x_next = x_curr + v_curr * ssl::config::TIME_STEP_SEC + 0.5 * acc_cmd * ssl::config::TIME_STEP_SEC
* ssl::config::TIME_STEP_SEC;
obstacle.circles.push_back (Circle_2 (Point_2 (x_next.x (), x_next.y ()), ssl::config::ROBOT_BOUNDING_RADIUS));
//TODO fill in between these circles with a rectangular polygon
// skipping this for now since, a robot in 20ms can move at most 6cm while moving with 3m/s
return obstacle;
}
void MapWidget::tfListenerProcess()
{
QPointF position;
double heading = 0.0;
bool shadow_changed = false;
std::stringstream stream;
while (ros::ok() && !tf_listener_thread_.interruption_requested())
{
if (getRobotPosition("/agent1/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent1", position, heading);
}
if (getRobotPosition("/agent2/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent2", position, heading);
}
if (getRobotPosition("/agent3/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent3", position, heading);
}
if (getRobotPosition("/agent4/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent4", position, heading);
}
if (getRobotPosition("/agent5/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent5", position, heading);
}
if (getRobotPosition("/agent6/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent6", position, heading);
}
if (getRobotPosition("/agent7/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent7", position, heading);
}
if (getRobotPosition("/agent8/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent8", position, heading);
}
if (getRobotPosition("/agent9/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent9", position, heading);
}
if (getRobotPosition("/agent10/base_link", position, heading)) {
drawShadow(position,heading);
shadow_changed = true;
emit tfChangedSignal("agent10", position, heading);
}
if (true == shadow_changed)
{
shadow_changed = false;
emit mapChangedSignal(map_shadow_);
}
boost::this_thread::sleep(boost::posix_time::milliseconds(200));
}//while
}
