diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index c29a30ca0..7743af2f3 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -41,4 +41,4 @@ objects: - type: urdf name: lander uri: package://mrover/urdf/world/lander.urdf.xacro - translation: [10, 10, 10] + translation: [8.5, 0.5, 0] diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp index 9daaa0d0b..caed2b778 100644 --- a/src/teleoperation/lander_align/lander_align.cpp +++ b/src/teleoperation/lander_align/lander_align.cpp @@ -61,7 +61,7 @@ namespace mrover { void LanderAlignNodelet::LanderCallback(sensor_msgs::PointCloud2Ptr const& cloud) { filterNormals(cloud); - ransac(0.2, 10, 100); + ransac(0.1, 10, 100); if (mBestNormalInZED.has_value()) { geometry_msgs::Vector3 vect; vect.x = mBestNormalInZED.value().x(); @@ -213,7 +213,7 @@ namespace mrover { if(mBestNormalInZED.value().x() > 0) mBestNormalInZED.value() *=-1; - mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + mBestNormalInZED.value()); + mBestOffsetInZED = std::make_optional(mBestLocationInZED.value() + 2 * mBestNormalInZED.value()); uploadPC(numInliers, distanceThreshold); @@ -314,7 +314,7 @@ namespace mrover { //Threhsolds float const linear_thresh = 0.1; // could be member variables - float const angular_thresh = 0.005; + float const angular_thresh = 0.001; PID pid(0.1, 0.1); // literally just P -- ugly class and probably needs restructuring in the future @@ -364,6 +364,8 @@ namespace mrover { double angle_rate = mAngleP * SO3tan.z(); + + ROS_INFO("w_z velocity %f", SO3tan.z()); @@ -387,7 +389,9 @@ namespace mrover { twist.linear.x = 0; } - double driveRate = mLinearP * distanceToTarget; + double driveRate = std::min(mLinearP * distanceToTarget, 1.0); + + ROS_INFO("distance: %f", distanceToTarget); twist.linear.x = driveRate; @@ -419,7 +423,7 @@ namespace mrover { if (std::abs(SO3tan.z()) < angular_thresh) { mLoopState = RTRSTATE::done; twist.angular.z = 0; - twist.linear.x = angle_rate; + twist.linear.x = 0; // ROS_INFO("Done turning to lander"); } diff --git a/src/teleoperation/lander_align/lander_align.hpp b/src/teleoperation/lander_align/lander_align.hpp index d48f6f968..5991f0ce0 100644 --- a/src/teleoperation/lander_align/lander_align.hpp +++ b/src/teleoperation/lander_align/lander_align.hpp @@ -27,7 +27,7 @@ namespace mrover { //PID CONSTANTS double const mAngleP = 1; - double const mLinearP = 0.1; + double const mLinearP = 0.3; ros::NodeHandle mNh, mPnh; ros::Publisher mDebugVectorPub; diff --git a/urdf/meshes/lander.fbx b/urdf/meshes/lander.fbx index a25c775e3..9afebcec6 100644 --- a/urdf/meshes/lander.fbx +++ b/urdf/meshes/lander.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fab8ba4467811dcee2efd5256a88c3dada38f66da36c02544d23b19cd2c05636 -size 489356 +oid sha256:9ae7d51a81922c7d4ea01e3abdf79ff8b2ad733bee6294d71cd287cec056f5da +size 490268 diff --git a/urdf/staging/lander.blend b/urdf/staging/lander.blend index 71fcd340b..2cf538d52 100644 --- a/urdf/staging/lander.blend +++ b/urdf/staging/lander.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d23056f5ba9f7bd3a1c6ccf8375b1613495a466f3f4e9ce347da0c3730c0aee7 -size 2011888 +oid sha256:c403cc5bf23baf1166e1cefc162177cff07226dc80c2e20f93ccf62a14be69e9 +size 1991040