diff --git a/app/freeFloorViewer/conf/freeFloorViewer_R1SN003.ini b/app/freeFloorViewer/conf/freeFloorViewer_R1SN003.ini index 1246ab78..d0876c82 100644 --- a/app/freeFloorViewer/conf/freeFloorViewer_R1SN003.ini +++ b/app/freeFloorViewer/conf/freeFloorViewer_R1SN003.ini @@ -34,6 +34,10 @@ navigation_server /navigation2D_nws_yarp map_locations_server /map2D_nws_yarp localization_server /localization2D_nws_yarp +[BASE_CONTROL] +base_ctrl_port /freeFloorViewer/baseVelCmd:o +max_angular_vel 20.0 + [POINTCLOUD_QUALITY] x_step 4 y_step 4 diff --git a/app/freeFloorViewer/conf/freeFloorViewer_R1_SIM.ini b/app/freeFloorViewer/conf/freeFloorViewer_R1_SIM.ini new file mode 100644 index 00000000..ee2fb664 --- /dev/null +++ b/app/freeFloorViewer/conf/freeFloorViewer_R1_SIM.ini @@ -0,0 +1,60 @@ +publish_ROS_pointcloud +thread_period 0.1 +period 0.1 +rpc_port /freeFloorViewer/rpc +clicked_pos_port /freeFloorViewer/clicked_pos:i +target_pos_port /freeFloorViewer/target:o +img_out_port /freeFloorViewer/floorEnhanced:o +self_reliant 0 + +[ROS] +useROS true +ROS_nodeName /cer-laserRGBD +ROS_topicName /laserDepth +frame_id /mobile_base_double_lidar + +[SENSOR] +resolution 0.5 + +[RGBD_SENSOR_CLIENT] +device RGBDSensorClient +localImagePort /clientRgbPort:i +localDepthPort /clientDepthPort:i +localRpcPort /clientRpcPort +remoteImagePort /SIM_CER_ROBOT/depthCamera/rgbImage:o +remoteDepthPort /SIM_CER_ROBOT/depthCamera/depthImage:o +remoteRpcPort /SIM_CER_ROBOT/depthCamera/rpc:i +ImageCarrier mjpeg +DepthCarrier fast_tcp+send.portmonitor+file.depthimage_compression_zlib+recv.portmonitor+file.depthimage_compression_zlib+type.dll + +[NAVIGATION_CLIENT] +device navigation2D_nwc_yarp +local /freeFloorViewer/navClient +navigation_server /navigation2D_nws_yarp +map_locations_server /map2D_nws_yarp +localization_server /localization2D_nws_yarp + +[BASE_CONTROL] +base_ctrl_port /freeFloorViewer/baseVelCmd:o +base_ctrl_state_port /freeFloorViewer/baseCtrlCmd:i +max_angular_vel 20.0 +max_linear_vel 0.5 + +[POINTCLOUD_QUALITY] +x_step 4 +y_step 4 + +[HEAD_CONTROL_CLIENT] +device remote_controlboard +local /freeFloorViewer/head +remote /SIM_CER_ROBOT/head + +[Z_CLIPPING_PLANES] +floor_height 0.1 +ceiling_height 2.1 +column_granularity 5 +camera_frame_id depth_center +ground_frame_id ground_link +extern_ref_frame_id map + + diff --git a/app/freeFloorViewer/conf/freeFloorViewer_complete_R1SN003.ini b/app/freeFloorViewer/conf/freeFloorViewer_complete_R1SN003.ini new file mode 100644 index 00000000..9a0d429d --- /dev/null +++ b/app/freeFloorViewer/conf/freeFloorViewer_complete_R1SN003.ini @@ -0,0 +1,60 @@ +publish_ROS_pointcloud +thread_period 0.1 +period 0.1 +rpc_port /freeFloorViewer/rpc +clicked_pos_port /freeFloorViewer/clicked_pos:i +target_pos_port /freeFloorViewer/target:o +img_out_port /freeFloorViewer/floorEnhanced:o +self_reliant 0 + +[ROS] +useROS true +ROS_nodeName /cer-laserRGBD +ROS_topicName /laserDepth +frame_id /mobile_base_double_lidar + +[SENSOR] +resolution 0.5 + +[RGBD_SENSOR_CLIENT] +device RGBDSensorClient +localImagePort /clientRgbPort:i +localDepthPort /clientDepthPort:i +localRpcPort /clientRpcPort +remoteImagePort /yarpOpenPose/image:o +remoteDepthPort /cer/realsense_repeater/depthImage:o +remoteRpcPort /cer/realsense_repeater/rpc:i +ImageCarrier mjpeg +DepthCarrier fast_tcp+send.portmonitor+file.depthimage_compression_zlib+recv.portmonitor+file.depthimage_compression_zlib+type.dll + +[NAVIGATION_CLIENT] +device navigation2D_nwc_yarp +local /freeFloorViewer/navClient +navigation_server /navigation2D_nws_yarp +map_locations_server /map2D_nws_yarp +localization_server /localization2D_nws_yarp + +[BASE_CONTROL] +base_ctrl_port /freeFloorViewer/baseVelCmd:o +base_ctrl_state_port /freeFloorViewer/baseCtrlCmd:i +max_angular_vel 20.0 +max_linear_vel 0.5 + +[POINTCLOUD_QUALITY] +x_step 4 +y_step 4 + +[HEAD_CONTROL_CLIENT] +device remote_controlboard +local /freeFloorViewer/head +remote /cer/head + +[Z_CLIPPING_PLANES] +floor_height 0.2 +ceiling_height 2.1 +column_granularity 5 +camera_frame_id extra_camera_tf +ground_frame_id ground_link +extern_ref_frame_id map + + diff --git a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003_wifi.xml b/app/freeFloorViewer/scripts/freeFloorViewer_R1SN003.xml similarity index 59% rename from app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003_wifi.xml rename to app/freeFloorViewer/scripts/freeFloorViewer_R1SN003.xml index f9aef223..77a7d9ee 100644 --- a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003_wifi.xml +++ b/app/freeFloorViewer/scripts/freeFloorViewer_R1SN003.xml @@ -1,25 +1,20 @@ -freeFloorViewer_testHead_R1SN003_wifi +freeFloorViewer_R1SN003 - - Navigation_ROS_R1SN003 - - - freeFloorViewer - --context freeFloorViewer --from freeFloorViewer_R1SN003.ini - YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=20000 - console3 + --context freeFloorViewer --from freeFloorViewer_complete_R1SN003.ini + YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=21000 + console2 yarpview --name /view/floorView:i --out /view/floorClick:o --rightout /view/headClick:o - console3 + console2 @@ -31,13 +26,25 @@ /view/floorClick:o /freeFloorViewer/clicked_pos:i - tcp + fast_tcp /view/headClick:o /freeFloorViewer/clicked_head:i - tcp + fast_tcp + + + + /freeFloorViewer/baseVelCmd:o + /baseControl/input/joystick:i + fast_tcp + + + + /baseControl/filtered_commands:o + /freeFloorViewer/baseCtrlCmd:i + fast_tcp diff --git a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim.xml b/app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM.xml similarity index 63% rename from app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim.xml rename to app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM.xml index dc698aec..1363e8c7 100644 --- a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim.xml +++ b/app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM.xml @@ -1,18 +1,13 @@ -freeFloorViewer_testHead_sim +freeFloorViewer_R1_SIM - - Navigation_ROS_R1_SIM - - - freeFloorViewer - --context freeFloorViewer --from freeFloorViewer_sim_tour.ini - YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=20000 + --context freeFloorViewer --from freeFloorViewer_R1_SIM.ini + YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=21000 console @@ -31,13 +26,25 @@ /view/floorClick:o /freeFloorViewer/clicked_pos:i - tcp + fast_tcp /view/headClick:o /freeFloorViewer/clicked_head:i - tcp + fast_tcp + + + + /freeFloorViewer/baseVelCmd:o + /baseControl/input/joystick:i + fast_tcp + + + + /baseControl/filtered_commands:o + /freeFloorViewer/baseCtrlCmd:i + fast_tcp diff --git a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003.xml b/app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM_DISTRIBUTED.xml similarity index 72% rename from app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003.xml rename to app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM_DISTRIBUTED.xml index 38baecbb..8e2914d1 100644 --- a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_R1SN003.xml +++ b/app/freeFloorViewer/scripts/freeFloorViewer_R1_SIM_DISTRIBUTED.xml @@ -1,18 +1,13 @@ -freeFloorViewer_testHead_R1SN003 +freeFloorViewer_R1_SIM_DISTRIBUTED - - Navigation_ROS_R1SN003 - - - freeFloorViewer - --context freeFloorViewer --from freeFloorViewer_R1SN003.ini - YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=20000 + --context freeFloorViewer --from freeFloorViewer_R1_SIM.ini + YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=21000 console2 @@ -31,13 +26,19 @@ /view/floorClick:o /freeFloorViewer/clicked_pos:i - tcp + fast_tcp /view/headClick:o /freeFloorViewer/clicked_head:i - tcp + fast_tcp + + + + /freeFloorViewer/baseVelCmd:o + /baseControl/input/joystick:i + fast_tcp diff --git a/app/freeFloorViewer/scripts/freeFloorViewer_complete_R1SN003.xml b/app/freeFloorViewer/scripts/freeFloorViewer_complete_R1SN003.xml new file mode 100644 index 00000000..6fabbaa4 --- /dev/null +++ b/app/freeFloorViewer/scripts/freeFloorViewer_complete_R1SN003.xml @@ -0,0 +1,22 @@ + +freeFloorViewer_complete_R1SN003 + + + + + + Navigation_ROS_R1SN003 + + + + + open_pose_stuff + + + + + freeFloorViewer_R1SN003 + + + + diff --git a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim_DISTRIBUTED.xml b/app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim_DISTRIBUTED.xml deleted file mode 100644 index 8e96ec3b..00000000 --- a/app/freeFloorViewer/scripts/freeFloorViewer_testHead_sim_DISTRIBUTED.xml +++ /dev/null @@ -1,43 +0,0 @@ - -freeFloorViewer_testHead_sim_DISTRIBUTED - - - - - - Navigation_ROS_R1_SIM_DISTRIBUTED - - - - - freeFloorViewer - --context freeFloorViewer --from freeFloorViewer_sim_tour.ini - YARP_PORTNUMBER_freeFloorViewer_floorEnhanced_o=20000 - console2 - - - - yarpview - --name /view/floorView:i --out /view/floorClick:o --rightout /view/headClick:o - console3 - - - - /freeFloorViewer/floorEnhanced:o - /view/floorView:i - mjpeg - - - - /view/floorClick:o - /freeFloorViewer/clicked_pos:i - tcp - - - - /view/headClick:o - /freeFloorViewer/clicked_head:i - tcp - - - diff --git a/app/freeFloorViewer/scripts/traefik.xml b/app/freeFloorViewer/scripts/traefik.xml index 2a9b995f..6be3cbd9 100644 --- a/app/freeFloorViewer/scripts/traefik.xml +++ b/app/freeFloorViewer/scripts/traefik.xml @@ -1,5 +1,5 @@ - traefik for webgui teleop + traefik_for_webgui_teleop @@ -23,7 +23,7 @@ python3 - server.py --map_port 20001 --camera_port 20000 --camera_host webgui.docker --map_host webgui.docker --click_port /click --traefik + server.py --map_port 20001 --camera_port 20000 --camera_host 88.54.208.58 --map_host 88.54.208.58 --no_ssl /home/user1/yarp-web-teleop traefik @@ -36,7 +36,7 @@ /freeFloorViewer/floorEnhanced:o /r1 - tcp + mjpeg @@ -48,18 +48,18 @@ /webview/headClick:o /freeFloorViewer/clicked_head:i - tcp + fast_tcp /webview/mapClick:o /navigationGui/yarpviewTarget:i - tcp + fast_tcp /webview/navClick:o /freeFloorViewer/clicked_pos:i - tcp + fast_tcp diff --git a/app/freeFloorViewer/scripts/webTeleopCompleteR1SN003.xml b/app/freeFloorViewer/scripts/webTeleopCompleteR1SN003.xml new file mode 100644 index 00000000..21adda38 --- /dev/null +++ b/app/freeFloorViewer/scripts/webTeleopCompleteR1SN003.xml @@ -0,0 +1,16 @@ + + web_teleop_complete_R1SN003 + + + + + freeFloorViewer_complete_R1SN003 + + + + + traefik_for_webgui_teleop + + + + diff --git a/app/freeFloorViewer/scripts/webTeleopCompleteSIM.xml b/app/freeFloorViewer/scripts/webTeleopCompleteSIM.xml new file mode 100644 index 00000000..13cc637d --- /dev/null +++ b/app/freeFloorViewer/scripts/webTeleopCompleteSIM.xml @@ -0,0 +1,22 @@ + +web_teleop_complete_SIM + + + + + + Navigation_ROS_R1_SIM + + + + + freeFloorViewer_R1_SIM + + + + + web_teleop_server_SIM + + + + diff --git a/app/freeFloorViewer/scripts/webTeleopCompleteSIM_DISTRIBUTED.xml b/app/freeFloorViewer/scripts/webTeleopCompleteSIM_DISTRIBUTED.xml new file mode 100644 index 00000000..1caa40ce --- /dev/null +++ b/app/freeFloorViewer/scripts/webTeleopCompleteSIM_DISTRIBUTED.xml @@ -0,0 +1,22 @@ + +web_teleop_complete_SIM_DISTRIBUTED + + + + + + Navigation_ROS_R1_SIM_DISTRIBUTED + + + + + freeFloorViewer_R1_SIM_DISTRIBUTED + + + + + web_teleop_server_SIM_DISTRIBUTED + + + + diff --git a/app/freeFloorViewer/scripts/webTeleopServerSIM.xml b/app/freeFloorViewer/scripts/webTeleopServerSIM.xml new file mode 100644 index 00000000..e0d3f45d --- /dev/null +++ b/app/freeFloorViewer/scripts/webTeleopServerSIM.xml @@ -0,0 +1,31 @@ + + web_teleop_server_SIM + + + + python3 + server.py --map_name /navigationGui/map:o --camera_name /freeFloorViewer/floorEnhanced:o --no_ssl + /home/user1/yarp-web-teleop + console + + + + + + /webview/headClick:o + /freeFloorViewer/clicked_head:i + fast_tcp + + + + /webview/mapClick:o + /navigationGui/yarpviewTarget:i + fast_tcp + + + + /webview/navClick:o + /freeFloorViewer/clicked_pos:i + fast_tcp + + diff --git a/app/freeFloorViewer/scripts/webTeleopServerSIM_DISTRIBUTED.xml b/app/freeFloorViewer/scripts/webTeleopServerSIM_DISTRIBUTED.xml new file mode 100644 index 00000000..217ccb9a --- /dev/null +++ b/app/freeFloorViewer/scripts/webTeleopServerSIM_DISTRIBUTED.xml @@ -0,0 +1,31 @@ + + web_teleop_server_SIM_DISTRIBUTED + + + + python3 + server.py --map_name /navigationGui/map:o --camera_name /freeFloorViewer/floorEnhanced:o --no_ssl + /home/user1/yarp-web-teleop + console1 + + + + + + /webview/headClick:o + /freeFloorViewer/clicked_head:i + fast_tcp + + + + /webview/mapClick:o + /navigationGui/yarpviewTarget:i + fast_tcp + + + + /webview/navClick:o + /freeFloorViewer/clicked_pos:i + fast_tcp + + diff --git a/src/freeFloorViewer/freeFloorThread.cpp b/src/freeFloorViewer/freeFloorThread.cpp index e95eec45..fbd16845 100644 --- a/src/freeFloorViewer/freeFloorThread.cpp +++ b/src/freeFloorViewer/freeFloorThread.cpp @@ -44,6 +44,8 @@ FreeFloorThread::FreeFloorThread(double _period, yarp::os::ResourceFinder &rf): m_extern_ref_frame_id = ""; m_imgOutPortName = "/freeFloorViewer/floorEnhanced:o"; m_targetOutPortName = "/free_floor_viewer/target:o"; + m_baseCmdOutPortName = "/freeFloorViewer/baseVelCmd:o"; + m_baseCmdInPortName = "/freeFloorViewer/baseCtrlCmd:i"; } bool FreeFloorThread::threadInit() @@ -57,6 +59,18 @@ bool FreeFloorThread::threadInit() if(m_rf.check("img_out_port")) {m_imgOutPortName = m_rf.find("img_out_port").asString();} if(m_rf.check("self_reliant")) {m_self_reliant = m_rf.find("self_reliant").asInt()==1;} + // --------- BaseControl config --------- // + bool okBaseCtrl = m_rf.check("BASE_CONTROL"); + if(okBaseCtrl){ + if(m_rf.check("base_ctrl_port")) {m_baseCmdOutPortName = m_rf.find("base_ctrl_port").asString();} + if(m_rf.check("base_ctrl_state_port")) {m_baseCmdInPortName = m_rf.find("base_ctrl_state_port").asString();} + if(m_rf.check("max_angular_vel")) {m_maxVelTheta = m_rf.find("max_angular_vel").asFloat64();} + if(m_rf.check("max_linear_vel")) {m_maxVelX = m_rf.find("max_linear_vel").asFloat64();} + m_outputBaseData.vel_x = 0.0; + m_outputBaseData.vel_y = 0.0; + m_outputBaseData.vel_theta = 0.0; + } + // --------- Z related props -------- // bool okZClipRf = m_rf.check("Z_CLIPPING_PLANES"); if(okZClipRf) @@ -216,8 +230,22 @@ bool FreeFloorThread::threadInit() yCInfo(FREE_FLOOR_THREAD) << "Depth Intrinsics:" << m_propIntrinsics.toString(); m_intrinsics.fromProperty(m_propIntrinsics); - m_imgOutPort.open(m_imgOutPortName); - m_targetOutPort.open(m_targetOutPortName); + if(!m_imgOutPort.open(m_imgOutPortName)){ + yCError(FREE_FLOOR_THREAD) << "Cannot open imgOut port with name" << m_imgOutPortName; + return false; + } + if(!m_targetOutPort.open(m_targetOutPortName)){ + yCError(FREE_FLOOR_THREAD) << "Cannot open targetOut port with name" << m_targetOutPortName; + return false; + } + if(!m_baseCmdOutPort.open(m_baseCmdOutPortName)){ + yCError(FREE_FLOOR_THREAD) << "Cannot open baseCmdOut port with name" << m_baseCmdOutPortName; + return false; + } + if(!m_baseCmdInPort.open(m_baseCmdInPortName)){ + yCError(FREE_FLOOR_THREAD) << "Cannot open baseCmdIn port with name" << m_baseCmdInPortName; + return false; + } #ifdef FREEFLOOR_DEBUG yCDebug(FREE_FLOOR_THREAD, "... done!\n"); @@ -282,14 +310,8 @@ void FreeFloorThread::run() m_floorMutex.lock(); //compute the point cloud - auto t1 = std::chrono::high_resolution_clock::now(); depthToFilteredPc(); - auto t2 = std::chrono::high_resolution_clock::now(); freeFloorDraw(imgOut); - auto t3 = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( t2 - t1 ).count(); - auto duration2 = std::chrono::duration_cast( t3 - t2 ).count(); - //yCInfo(FREE_FLOOR_THREAD) << "The durations are: " << duration << " " << duration2; m_floorMutex.unlock(); @@ -352,7 +374,12 @@ void FreeFloorThread::freeFloorDraw(yarp::sig::ImageOf &ou double arScaler = 0.2; yarp::dev::Nav2D::NavigationStatusEnum currentStatus; m_iNav2D->getNavigationStatus(currentStatus); - bool moving = currentStatus == yarp::dev::Nav2D::navigation_status_moving; + yarp::os::Bottle* baseCmdIn = m_baseCmdInPort.read(false); + bool navigating = currentStatus == yarp::dev::Nav2D::navigation_status_moving; + bool moving = false; + if (baseCmdIn){ + moving = (baseCmdIn->get(0).asFloat64() != 0.0) || (baseCmdIn->get(1).asFloat64() != 0.0); + } //rotateAndCheck(m_pc, m_transform_mtrx,m_rgbImage,imgOut,m_okPixels,m_floor_height,m_ceiling_height); output.copy(m_rgbImage); @@ -362,9 +389,9 @@ void FreeFloorThread::freeFloorDraw(yarp::sig::ImageOf &ou int u = blob.first.first; int v = blob.first.second; m_okPixels.push_back(blob.first); - pOk.r = moving ? output.pixel(u,v).r*arScaler+255*(1-arScaler) : output.pixel(u,v).r*arScaler; - pOk.b = output.pixel(u,v).b*arScaler; - pOk.g = moving ? output.pixel(u,v).g*arScaler : output.pixel(u,v).g*arScaler+255*(1-arScaler); + pOk.r = (!navigating && moving) ? output.pixel(u,v).r*arScaler+255*(1-arScaler) : output.pixel(u,v).r*arScaler; + pOk.b = (navigating) ? output.pixel(u,v).b*arScaler+255*(1-arScaler) : output.pixel(u,v).b*arScaler; + pOk.g = (navigating || moving) ? output.pixel(u,v).g*arScaler : output.pixel(u,v).g*arScaler+255*(1-arScaler); output.pixel(u,v) = pOk; } @@ -396,6 +423,13 @@ void FreeFloorThread::onRead(yarp::os::Bottle &b) { reachSpot(b); } + else if(b.size()==5){ + if(b.get(0).asString() != "base"){ + yCError(FREE_FLOOR_THREAD) << "The first element of the bottle should be \"base\" but it's actually:" << b.get(0).asString(); + return; + } + moveBase(b); + } else if(b.size() == 4) { rotate(b); @@ -460,7 +494,8 @@ void FreeFloorThread::rotate(yarp::os::Bottle &b) } int deltaPx = (b.get(0).asInt32()-b.get(2).asInt32()); - double rotation = (double)deltaPx * horizFOV/m_depth_width; + //double rotation = (double)deltaPx * horizFOV/m_depth_width; + double rotation = (double)deltaPx * 180.0/m_depth_width; //int sign = (b.get(0).asInt32()-b.get(2).asInt32())/(abs(b.get(0).asInt32()-b.get(2).asInt32())>0?abs(b.get(0).asInt32()-b.get(2).asInt32()):1); if(m_nav2DPoly.isValid()) { @@ -475,6 +510,44 @@ void FreeFloorThread::rotate(yarp::os::Bottle &b) } } +void FreeFloorThread::moveBase(yarp::os::Bottle& b) +{ + bool leftRight = false; + bool upDown = false; + if(b.get(1).asInt32() != 0 && b.get(2).asInt32() == 0){ + m_outputBaseData.vel_theta = (double)b.get(1).asInt32()*(m_maxVelTheta)/100.0; + } + else if(b.get(2).asInt32() != 0 && b.get(1).asInt32() == 0){ + m_outputBaseData.vel_theta = (double)b.get(2).asInt32()*(m_maxVelTheta)/100.0*(-1.0); + } + else if(b.get(2).asInt32() != 0 && b.get(1).asInt32() != 0){ + yCError(FREE_FLOOR_THREAD) << "You cannot go both left and right"; + leftRight = true; + } + else{ + m_outputBaseData.vel_theta = 0.0; + } + if(b.get(3).asInt32() != 0 && b.get(4).asInt32() == 0){ + m_outputBaseData.vel_x = (double)b.get(3).asInt32()*(m_maxVelX)/100.0; + } + else if(b.get(4).asInt32() != 0 && b.get(3).asInt32() == 0){ + m_outputBaseData.vel_x = (double)b.get(2).asInt32()*(m_maxVelX)/100.0*(-1.0); + } + else if(b.get(4).asInt32() != 0 && b.get(3).asInt32() != 0){ + yCError(FREE_FLOOR_THREAD) << "You cannot go both forward and backward"; + upDown = true; + } + else{ + m_outputBaseData.vel_x = 0.0; + } + if(upDown || leftRight){ + return; + } + m_baseCmdOutPort.write(m_outputBaseData); + m_currentVelTheta = m_outputBaseData.vel_theta; + m_currentVelX = m_outputBaseData.vel_x; +} + void FreeFloorThread::threadRelease() { #ifdef FREEFLOOR_DEBUG diff --git a/src/freeFloorViewer/freeFloorThread.h b/src/freeFloorViewer/freeFloorThread.h index 31b4b16f..e5948fb8 100644 --- a/src/freeFloorViewer/freeFloorThread.h +++ b/src/freeFloorViewer/freeFloorThread.h @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -80,6 +81,10 @@ class FreeFloorThread : public yarp::os::PeriodicThread, public yarp::os::TypedR bool m_self_reliant{true}; double m_floor_height; double m_ceiling_height; + double m_maxVelTheta{10.0}; + double m_maxVelX{0.1}; + double m_currentVelTheta{0.0}; + double m_currentVelX{0.0}; size_t m_pc_stepx; size_t m_pc_stepy; std::string m_ground_frame_id; @@ -92,6 +97,7 @@ class FreeFloorThread : public yarp::os::PeriodicThread, public yarp::os::TypedR yarp::sig::utils::PCL_ROI m_pc_roi; yarp::sig::ImageOf m_depth_image; yarp::sig::IntrinsicParams m_intrinsics; + yarp::dev::MobileBaseVelocity m_outputBaseData; yarp::sig::PointCloud m_pc; std::vector> m_okPixels; std::map,bool> m_obstacle_columns; @@ -100,7 +106,11 @@ class FreeFloorThread : public yarp::os::PeriodicThread, public yarp::os::TypedR //Ports std::string m_targetOutPortName; std::string m_imgOutPortName; + std::string m_baseCmdOutPortName; + std::string m_baseCmdInPortName; yarp::os::BufferedPort m_targetOutPort; + yarp::os::Port m_baseCmdOutPort; + yarp::os::BufferedPort m_baseCmdInPort; yarp::os::BufferedPort> m_imgOutPort; //Others @@ -123,6 +133,7 @@ class FreeFloorThread : public yarp::os::PeriodicThread, public yarp::os::TypedR //Internal methods void reachSpot(yarp::os::Bottle& b); void rotate(yarp::os::Bottle& b); + void moveBase(yarp::os::Bottle& b); void freeFloorDraw(yarp::sig::ImageOf &output); void depthToFilteredPc();