diff --git a/src/node.cpp b/src/node.cpp index 1e770d3fe77447c216573b72e1d36d29cb57dd0c..d51a3d93278e125436ba8aa0371a785295333e08 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -116,8 +116,8 @@ bool WolfRosNode::updateTf() //Get map2base from Wolf result, and builds base2map pose tf::Transform T_map2base; - if (current_state.count("P") == 0 or - current_state.count("O") == 0 or + if (current_state.count('P') == 0 or + current_state.count('O') == 0 or !loc_ts.ok()) { if (state_available_) @@ -137,14 +137,14 @@ bool WolfRosNode::updateTf() // 2D if (problem_ptr_->getDim() == 2) { - T_map2base = tf::Transform (tf::createQuaternionFromYaw(current_state["O"](0)), - tf::Vector3(current_state["P"](0), current_state["P"](1), 0) ); + T_map2base = tf::Transform (tf::createQuaternionFromYaw(current_state['O'](0)), + tf::Vector3(current_state['P'](0), current_state['P'](1), 0) ); } // 3D else { - T_map2base = tf::Transform (tf::Quaternion(current_state["O"](0), current_state["O"](1), current_state["O"](2), current_state["O"](3)), - tf::Vector3(current_state["P"](0), current_state["P"](1), current_state["P"](2)) ); + T_map2base = tf::Transform (tf::Quaternion(current_state['O'](0), current_state['O'](1), current_state['O'](2), current_state['O'](3)), + tf::Vector3(current_state['P'](0), current_state['P'](1), current_state['P'](2)) ); } } diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp index ee95b54b2e4673ac9374685791024b1096b4f0f5..766c07c621046c97880ed321f027d2a3a597420b 100644 --- a/src/publisher_pose.cpp +++ b/src/publisher_pose.cpp @@ -29,8 +29,8 @@ void PublisherPose::publishDerived() TimeStamp loc_ts = problem_->getTimeStamp(); // state not ready - if (current_state.count("P") == 0 or - current_state.count("O") == 0 or + if (current_state.count('P') == 0 or + current_state.count('O') == 0 or loc_ts == TimeStamp(0)) { return; @@ -44,23 +44,23 @@ void PublisherPose::publishDerived() // 2D if (problem_->getDim() == 2) { - msg.pose.pose.position.x = current_state["P"](0); - msg.pose.pose.position.y = current_state["P"](1); + msg.pose.pose.position.x = current_state['P'](0); + msg.pose.pose.position.y = current_state['P'](1); msg.pose.pose.position.z = 0; - msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_state["O"](0)); + msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_state['O'](0)); } // 3D else { - msg.pose.pose.position.x = current_state["P"](0); - msg.pose.pose.position.y = current_state["P"](1); - msg.pose.pose.position.z = current_state["P"](2); + msg.pose.pose.position.x = current_state['P'](0); + msg.pose.pose.position.y = current_state['P'](1); + msg.pose.pose.position.z = current_state['P'](2); - msg.pose.pose.orientation.x = current_state["O"](0); - msg.pose.pose.orientation.y = current_state["O"](1); - msg.pose.pose.orientation.z = current_state["O"](2); - msg.pose.pose.orientation.w = current_state["O"](3); + msg.pose.pose.orientation.x = current_state['O'](0); + msg.pose.pose.orientation.y = current_state['O'](1); + msg.pose.pose.orientation.z = current_state['O'](2); + msg.pose.pose.orientation.w = current_state['O'](3); } publisher_.publish(msg); diff --git a/src/visualizer.cpp b/src/visualizer.cpp index b8f81a61a391d4a388252ec4525c4b72e50aeb63..e1a717833c9a0ea469ef6fd488310b238d3762a0 100644 --- a/src/visualizer.cpp +++ b/src/visualizer.cpp @@ -32,32 +32,32 @@ void Visualizer::initialize(ros::NodeHandle& nh) nh.param<float>( "frame_color_r", frame_color_.r, 1.0); nh.param<float>( "frame_color_g", frame_color_.g, 0.8); nh.param<float>( "frame_color_b", frame_color_.b, 0.0); - nh.param<float>( "frame_color_a", frame_color_.a, 0.5); + nh.param<float>( "frame_color_a", frame_color_.a, 1); nh.param<float>( "factor_abs_color_r", factor_abs_color_.r, 0.92); nh.param<float>( "factor_abs_color_g", factor_abs_color_.g, 0.19); nh.param<float>( "factor_abs_color_b", factor_abs_color_.b, 0.6); - nh.param<float>( "factor_abs_color_a", factor_abs_color_.a, 0.5); + nh.param<float>( "factor_abs_color_a", factor_abs_color_.a, 1); nh.param<float>( "factor_motion_color_r", factor_motion_color_.r, 1.0); nh.param<float>( "factor_motion_color_g", factor_motion_color_.g, 1.0); nh.param<float>( "factor_motion_color_b", factor_motion_color_.b, 0.0); - nh.param<float>( "factor_motion_color_a", factor_motion_color_.a, 0.5); + nh.param<float>( "factor_motion_color_a", factor_motion_color_.a, 1); nh.param<float>( "factor_loop_color_r", factor_loop_color_.r, 1.0); nh.param<float>( "factor_loop_color_g", factor_loop_color_.g, 0.0); nh.param<float>( "factor_loop_color_b", factor_loop_color_.b, 0.0); - nh.param<float>( "factor_loop_color_a", factor_loop_color_.a, 0.5); + nh.param<float>( "factor_loop_color_a", factor_loop_color_.a, 1); nh.param<float>( "factor_lmk_color_r", factor_lmk_color_.r, 0.0); nh.param<float>( "factor_lmk_color_g", factor_lmk_color_.g, 0.0); nh.param<float>( "factor_lmk_color_b", factor_lmk_color_.b, 1.0); - nh.param<float>( "factor_lmk_color_a", factor_lmk_color_.a, 0.5); + nh.param<float>( "factor_lmk_color_a", factor_lmk_color_.a, 1); nh.param<float>( "factor_geom_color_r", factor_geom_color_.r, 0.0); nh.param<float>( "factor_geom_color_g", factor_geom_color_.g, 1.0); nh.param<float>( "factor_geom_color_b", factor_geom_color_.b, 1.0); - nh.param<float>( "factor_geom_color_a", factor_geom_color_.a, 0.5); + nh.param<float>( "factor_geom_color_a", factor_geom_color_.a, 1); // init markers --------------------------------------------------- // factor markers message