Merge remote-tracking branch 'origin/tjn/threejs_new' into arm-practice

umrover · Apr 20, 2024 · 3837e83 · 3837e83
2 parents ac5fe3b + 33d430c
commit 3837e83
Show file tree

Hide file tree

Showing 36 changed files with 505 additions and 203 deletions.
diff --git a/config/teleop.yaml b/config/teleop.yaml
@@ -119,6 +119,6 @@ teleop:
     bar: ["/tf_static", "/rosout", "/tf"]
 
   ik_multipliers:
-    x: 1
-    y: 1
-    z: 1
+    x: 0.1
+    y: 0.1
+    z: 0.1
diff --git a/launch/basestation.launch b/launch/basestation.launch
@@ -4,14 +4,17 @@
 <launch>
   <arg name="run_rviz" default="false" />
   <arg name="run_network_monitor" default="false" />
+  <arg name="run_backend" default="true" />
+  <arg name="run_frontend" default="true" />
+  <arg name="run_browser" default="true" />
 
   <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
   <rosparam command="load" file="$(find mrover)/config/teleop.yaml" />
   <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
 
-  <node name="gui_frontend" pkg="mrover" type="gui_frontend.sh" cwd="node" required="true" />
-  <node name="gui_backend" pkg="mrover" type="gui_backend.sh" cwd="node" required="true" />
-  <node name="gui_chromium" pkg="mrover" type="gui_chromium.sh" />
+  <node if="$(arg run_frontend)" name="gui_frontend" pkg="mrover" type="gui_frontend.sh" cwd="node" required="true" />
+  <node if="$(arg run_backend)" name="gui_backend" pkg="mrover" type="gui_backend.sh" cwd="node" required="true" />
+  <node if="$(arg run_browser)" name="gui_chromium" pkg="mrover" type="gui_chromium.sh" />
 
   <group if="$(arg run_rviz)">
     <arg name="rvizconfig" default="$(find mrover)/config/rviz/basestation.rviz" />

diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py
@@ -1,7 +1,8 @@
 #!/usr/bin/env python3
 
 import rospy
-from geometry_msgs.msg import Pose, Quaternion, Point, PointStamped
+from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, PointStamped
+from std_msgs.msg import Header
 from mrover.msg import IK
 
 if __name__ == "__main__":
@@ -13,18 +14,24 @@
 
     pub.publish(
         IK(
-            pose=Pose(
-                position=Point(x=0.5, y=0.5, z=0.5),
-                orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            target=PoseStamped(
+                header=Header(stamp=rospy.Time.now(), frame_id="base_link"),
+                pose=Pose(
+                    position=Point(x=0.8, y=1.0, z=0.5),
+                    orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+                ),
             )
         )
     )
 
     def on_clicked_point(clicked_point: PointStamped):
         ik = IK(
-            pose=Pose(
-                position=clicked_point.point,
-                orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            target=PoseStamped(
+                header=Header(stamp=rospy.Time.now(), frame_id="base_link"),
+                pose=Pose(
+                    position=clicked_point.point,
+                    orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+                ),
             )
         )
         pub.publish(ik)

diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp
@@ -1,4 +1,5 @@
 #include "simulator.hpp"
+#include <sensor_msgs/JointState.h>
 
 namespace mrover {
 
@@ -22,7 +23,7 @@ namespace mrover {
         mMotorStatusPub = mNh.advertise<MotorsStatus>("/drive_status", 1);
         mDriveControllerStatePub = mNh.advertise<ControllerState>("/drive_controller_data", 1);
         mArmControllerStatePub = mNh.advertise<ControllerState>("/arm_controller_data", 1);
-        mArmJointStatePub = mNh.advertise<sensor_msgs::JointState>("/arm_joint_states", 1);
+        mArmJointStatePub = mNh.advertise<sensor_msgs::JointState>("/arm_joint_data", 1);
 
         mIkTargetPub = mNh.advertise<IK>("/arm_ik", 1);
 

diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp
@@ -4,6 +4,7 @@
 
 #include "glfw_pointer.hpp"
 #include "wgpu_objects.hpp"
+#include <ros/publisher.h>
 
 using namespace std::literals;
 

diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp
@@ -208,6 +208,19 @@ namespace mrover {
         mWindow = GlfwPointer<GLFWwindow, glfwCreateWindow, glfwDestroyWindow>{w, h, WINDOW_NAME, nullptr, nullptr};
         NODELET_INFO_STREAM(std::format("Created window of size: {}x{}", w, h));
 
+        if (cv::Mat logo = imread(std::filesystem::path{std::source_location::current().file_name()}.parent_path() / "mrover_logo.png", cv::IMREAD_UNCHANGED);
+            logo.type() == CV_8UC4) {
+            cvtColor(logo, logo, cv::COLOR_BGRA2RGBA);
+            GLFWimage logoImage{
+                    .width = logo.cols,
+                    .height = logo.rows,
+                    .pixels = logo.data,
+            };
+            glfwSetWindowIcon(mWindow.get(), 1, &logoImage);
+        } else {
+            ROS_WARN_STREAM("Failed to load logo image");
+        }
+
         if (glfwRawMouseMotionSupported()) glfwSetInputMode(mWindow.get(), GLFW_RAW_MOUSE_MOTION, GLFW_TRUE);
 
         glfwSetWindowUserPointer(mWindow.get(), this);

diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp
@@ -1,4 +1,5 @@
 #include "simulator.hpp"
+#include <sensor_msgs/JointState.h>
 
 namespace mrover {
 
@@ -243,11 +244,19 @@ namespace mrover {
             mDriveControllerStatePub.publish(driveControllerState);
 
             ControllerState armControllerState;
+            sensor_msgs::JointState armJointState;
+            std::vector<double> zeros = {0, 0, 0, 0, 0, 0};
+            armJointState.header.stamp = ros::Time::now();
             for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) {
                 armControllerState.name.emplace_back(armMsgToUrdf.backward(linkName).value());
                 armControllerState.state.emplace_back("Armed");
                 armControllerState.error.emplace_back("None");
 
+                armJointState.name.emplace_back(armMsgToUrdf.backward(linkName).value());
+                armJointState.position.emplace_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index));
+                armJointState.velocity.emplace_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index));
+                armJointState.effort.emplace_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index));
+
                 std::uint8_t limitSwitches = 0b000;
                 if (auto limits = rover.model.getLink(linkName)->parent_joint->limits) {
                     double joinPosition = rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index);
@@ -258,16 +267,7 @@ namespace mrover {
                 armControllerState.limit_hit.push_back(limitSwitches);
             }
             mArmControllerStatePub.publish(armControllerState);
-
-            sensor_msgs::JointState jointState;
-            jointState.header.stamp = ros::Time::now();
-            for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) {
-                jointState.name.push_back(armMsgToUrdf.backward(linkName).value());
-                jointState.position.push_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index));
-                jointState.velocity.push_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index));
-                jointState.effort.push_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index));
-            }
-            mArmJointStatePub.publish(jointState);
+            mArmJointStatePub.publish(armJointState);
         }
     }
 

diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp
@@ -16,19 +16,19 @@ namespace mrover {
         Position positions;
         positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"};
         positions.positions.resize(positions.names.size());
-        SE3d target_frame_to_arm_b_static;
+        SE3d targetFrameToArmBaseLink;
         try {
-            target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link");
-        } catch (...) {
-            ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame");
+            targetFrameToArmBaseLink = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link");
+        } catch (tf2::TransformException const& exception) {
+            ROS_WARN_STREAM_THROTTLE(1, std::format("Failed to get transform from {} to arm_base_link: {}", ik_target.target.header.frame_id, exception.what()));
             return;
         }
-        Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1};
-        Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target;
-        double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector
-        double z = target_in_arm_b_static.z();
-        double y = target_in_arm_b_static.y();
-        SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static);
+        SE3d endEffectorInTarget{{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z}, SO3d::Identity()};
+        SE3d endEffectorInArmBaseLink = targetFrameToArmBaseLink * endEffectorInTarget;
+        double x = endEffectorInArmBaseLink.translation().x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector
+        double y = endEffectorInArmBaseLink.translation().y();
+        double z = endEffectorInArmBaseLink.translation().z();
+        SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", endEffectorInArmBaseLink);
 
         double gamma = 0;
         double x3 = x - LINK_DE * std::cos(gamma);

diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp
@@ -2,6 +2,7 @@
 
 #include <cmath>
 #include <numbers>
+#include <format>
 
 #include <Eigen/Core>
-Original file line number
+Diff line change
@@ Expand Up / @@ -119,6 +119,6 @@ teleop: @@
         bar: ["/tf_static", "/rosout", "/tf"]
       ik_multipliers:
-        x: 1
-        y: 1
-        z: 1
+        x: 0.1
+        y: 0.1
+        z: 0.1